Создание собственной системы стабилизации
После запуска программы, GPS буквально за 5сек потерял все спутники.
Ищите источник (паразитный) радиоизлучения на плате… Проблема актуальная, например некоторые видеокамеры при приближении к модулю GPS аналогично глушат сигнал…
Стало лучше после установки дросселя по +. Но все равно после запуска ядра МК теряется часть спутников.
в этом архивчике с экзамплами www.st.com/web/en/catalog/tools/PF257904
у меня только wirish_math из maple выдрано…
в библиотеках CMSIS точно есть arm_math.h если что тоже есть…
За ссылочки спасибо, но там только заголовочный файл с объявленниями, а вот где бы посмотреть реализацию…
Там все довольно просто, могу даже пример проекта скинуть на F4 или F103
я писал про F30х (в Coocox поддержки нет по умолчанию), а вы говорите про F4 и F1…
на пред-й странице я писал и выкладывал ссылку шаблон демо проекта под F3, но debug не пашет там…
но там только заголовочный файл с объявленниями, а вот где бы посмотреть реализацию…
поиском воспользуйтесь 😉
например функция arm_mat_add_f32
arm_status arm_mat_add_f32(
const arm_matrix_instance_f32 * pSrcA,
const arm_matrix_instance_f32 * pSrcB,
arm_matrix_instance_f32 * pDst);
путь в архиве: \STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\DSP_Lib\Source\MatrixFunctions\arm_mat_add_f32.c
Стало лучше после установки дросселя по +. Но все равно
Какая плата? Если неудачная топология разводки, то поможет полное экранирование (как у радиомодулей)…
переделал корректор ИНС как у Тома Пикке, code.google.com/p/gluonpilot/...e_quaternion.c
а по чем там X,Y корректится? гпс? optflow?
вся математика алгебраически оптимизирована, можешь на АВР попробовать
глянул… не потянет авр-ка похоже…
Счас горизонт лучше, но из-за наличия интегральной составляющей присутствует медленная раскачка.
Если ты про конечный пид регуль, то есть парочка рецептов:
- убери I в ноль, если станет заметно стабильнее, то подними его на мин., напару единиц, так что бы не влияло на горизонт и не вводило в раскачку…
- если у тебя классический пид регуль с углом из ИМУ на входе, то убедись что у тебя правильный знак на Д-часть и она задает противодействие (П+И)-частям. т.е. если на входе угол, то Д-часть это по сути угловая скорость, принимая что требуемое/заданное положение/угол постоянно на коротком промежутке времени… т.е. Д-часть должна вводить заблаговременное торможение системы, а не ускорение… Вообще имеют право на жизнь П+И+Д и П+И-Д регули, но первый вариант для коптеров не катит. Уже (не помню кто) проверял на форуме. Результат П+И+Д: раскачка и перевертыши…
Если со знаками в Д-части все ок, то попробый увеличить Дк, т.е. силу противодействия/торможения к требуемому/заданному положению…
Вот коротко и клева написано про пидЫ pidcontrol.narod.ru - Не помогло? Тогда далее 😃
Удали Д часть из ПИД регуля и введи ПД (со знаком минус!) по скорости с ДУС. Собственно это секрет стабильности вия в акро и стаб/level режимах.
Т.е. получится:
УВ(упр. воздействие) = ПИ(угол из ИМУ на входе) - ПД(угловая скорость гиры)
Для ПД (по скорости с гиры) есть парочка нюансов:
- данные с гиры надо слегка усреднить ~20 герц
- Д-часть, т.е. дифференциал угловой скорости - это по сути угловое ускорение, которое может начать шуметь, потому его тож надо усреднить чутка. В вие усредняются три последних дифф-а (median filter вроде). Если брать 4-ре, то выходит по плавности совсем “подводная лодка” 😃 и убирает мелкие осцилляции на концах лучей. У меня в прошивах - это KILL_SMALL_OSCILLATIONS. ФПВэшники рады 😃
/* It should help to remove small oscillations produced by D-part of main PID regulator.
* It's also usefull to improve vibro-protection in general, even stability for video cameras (info from feedback ot real user).
* Note: theoretically it increases D-part for 20-30% and you will need to increase acro P coef. accordingly.
* E.g. P roll/pitch was 5.2 and should be changed to 6-6.5
*/
//#define KILL_SMALL_OSCILLATIONS
Вот код ПД-части по гире:
PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
По настройке:
а) обнули ПИ по углу… по сути отруби результат своей ИМУ вращалки 😃
б) натяни ПД так (вплоть до появления перекомпенсации/осцилляций), чтобы в руках коптер существенно сопротивляся наклонам на газу висения (т.е. 40-60%)… потом уменьшай ПД на 20…30%, что бы не вводить в перекомпенсацию…
в) подымай П (по углу) на желаемую скорость реакции стиков… + добавь немного И, сосем чутка…
Должно помочь 😉
поцарапал стены, поругался с женой
моя за стены не бьет, а вот за измену ей с коптером ругается! 😃
Гдето видел OpticFlow за 50 баксов и не могу вспонить где
вот вариант красивый, но дороговато:
pixhawk.ethz.ch/px4/modules/px4flow
store.diydrones.com/ProductDetails.asp?ProductCode…
а по чем там X,Y корректится? гпс?
Скажите а ЧТО можно скорректировать по ГПС ? если точность при н.у. +/- 10 метров ?
Скажите а ЧТО можно скорректировать по ГПС ? если точность при н.у. +/- 10 метров ?
в теории да: через компл. фильтр, либо калман… в крутых контроллерах типа немец, наза, а также от vis.asta так и сделано… он иногда по капле открывает секреты 😃
чем точнее (и дороже соот-но) аксель, тем меньше можно брать степень корректировки, чтобы не вывалится из стабилизации интеграторов… он кстати, про mpu6050 (который мы тут так хвалим и радуемся) отзывался как про унылое ггг 😃 … каждой задаче своя цена как грится…
upd: вчера написал первую версию аксель+гпс 😃
как распогАдится, пойду тестить!
версию аксель+гпс
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
если абстрагироваться от ИМУ, то вот матрица поворота, только знаки надо подобрать правильно (тот еще ребус!):
#define deg2rad(x) (float)(x*PI/180.0f)
float cr = cos(deg2rad(angle[ROLL]));
float cp = cos(deg2rad(angle[PITCH]));
float cy = cos(deg2rad(angle[YAW]));
float sr = sin(deg2rad(angle[ROLL]));
float sp = sin(deg2rad(angle[PITCH]));
float sy = sin(deg2rad(angle[YAW]));
float earth_acc_x = (cp*cy) * acc_x + (sr*sp*cy - cr*sy) * acc_y + (sr*sy + cr*sp*cy) * acc_z;
float earth_acc_y = (cp*sy) * acc_x + (cr*cy + sr*sp*sy) * acc_y + (-sr*cy + cr*sp*sy) * acc_z;
float earth_acc_z = (-sp) * acc_x + (sr*cp) * acc_y + (cr*cp) * acc_z;
на выходе earth_acc_x/y/z - это ускорения не зависимые от наклонов…
Вы вроде с инерциалкой Александра (rual) играетесь. Там проще по идее, т.к. в кватернионах калькуляции…
Я уже писал вам, одного ICP(Input Capture Pin) хватит для обработки PPM(CPPM) сигнала. Для этого достаточно ловить только Rising Edge (из 0 в 1). Если вам надо каждый канал по отдельности обрабатывать, то разумеется вам надо столько ICP пинов, сколько у вас каналов и в этом случае надо его переключать из 0 в 1 и 1 в 0.
в обычном приемнике, от Futaba 7C например, РРМ сигнал ведь разложен по 7 канала, на каждой ножке свой канал…
в Atmega128 есть только 2 прерывания с режимом захвата, это 1 и 3 NIT, т.е. я могу только 2 канала с приемника аппаратно обработать. а что делать с остальными? на них нужно программный захват организовывать с помощью простых INTов?
Я так понял все тут делают на STM32, а у него сколько аппаратных ICP прерываний? Или есть кто делает на атмеге?
Ну понаписали!!! Вот куда время уходит!!!😛
Народ мне тут на ухо нашептали, грамотные программеры меня поправя если что не так, по прерываниям, первое что надо сделать в прерывании - это запретить все прерывания
Сергей, неверно тебе нашептали, но АВР по умолчанию именно так и делает, SirAlex ниже написал про эту особенность. В СТМ32 всё гораздо лучше, в нем оптимизирована смена контекста работы проца (переход в подпрограммы), есть векторный контроллер прерываний, вы бирай сам что для тебя более важно. Главное не забыть выделить достаточное место под стек! И не надо париться что там что прерывает, у меня по 7 прерываний “одновременно” обрабатываются:)
Ну понаписали!!! Вот куда время уходит!!!
ну да, ну да 😃
у нас завтра демо по проекту, а я через translit.ru, расписался тут 😃
вообще надеюсь поможет, ибо имея даже самый быстрый и точный IMU, можно получить хреновый результат, если выходной пид-регуль “слаб” 😃
в обычном приемнике, от Futaba 7C например, РРМ сигнал ведь разложен по 7 канала, на каждой ножке свой канал…
в Atmega128 есть только 2 прерывания с режимом захвата, это 1 и 3 NIT, т.е. я могу только 2 канала с приемника аппаратно обработать. а что делать с остальными? на них нужно программный захват организовывать с помощью простых INTов?
Я так понял все тут делают на STM32, а у него сколько аппаратных ICP прерываний? Или есть кто делает на атмеге?
Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.
Погоди, так PPM приходит же единым пакетом. Все каналы сразу. Значит тебе для его обработки нужна всего 1 ножка. И с помощью этой 1 ножки ты и обрабатываешь все свои 7 каналов. Тебе нужно ловить только переходы из 0 в 1 и из 1 в 0 и все, как написали выше.
да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…
вместо ифк датчиков прислали 4 пирометра, поругаться или с пирометрами что-нибудь замутить?
Это подстава, “прорекламируй” поставщика.
А поделитесь ссылкой на математику?
ищем по “расстояние между точками на поверхности земли”, там мног всего, для начала gis-lab.info/qa/great-circles.html см. реализацию для эсель (наиболее близко к Си). Из этого просто нужно сделать 2мерный вектор ХУ.
а по чем там X,Y корректится? гпс? optflow?
нет, это чистый горизонт, линейные перемещения пока сам планирю считать. Просто более простой и понятной (мне) реализации не встречал. Только у него все лишние члены векторных произведений типа (1,0,0)х(0,0,1) исключены, я же всё делал “академически” ибо изобретал “велосипед” с нуля (т.е. с высоты своих познаний)
глянул… не потянет авр-ка похоже…
А ты попробуй, ДЦМ помоему шибко хуже.
Вот код ПД-части по гире:
Я как раз пор это, ПИ-регулятор комплементарного филтра. Тут палка о двух концах, сделаешь большой И - будет перекомпенсация (горизонт будет крыльями махать), при маленьком И будет нескомпенсированый завал в одну из сторон.
в крутых контроллерах типа немец, наза, а также от vis.asta так и сделано…
Тут по другому никак, более оперативных перемещения датчиков нет
он кстати про mpu6050
нет, он кстати про МПУ6000 на ПХ4м контроллере.
вот вариант красивый, но дороговато
Да, это сказка, то что нужно для конкурса (только тссс!), вот только стоил бы он баксов 70. А так я сам такой собиру, нужно только дождаться цифровой камеры от китайцких товарищей. Я видел где-то на датчике от мыши, просто туда был прикручен объектив от камеры.
Ломаю голову, как бы разделить линейные ускорения и крен у акселя… пока глухо.
“Академически” делается предельно просто: поворачиваем вектор акселя в мировую СК и вычитаем вектор нормали (0,0,-1) или (0,0,1) зависит от “полярности” акселя. В остатке все остальные ускорения 😁, в т.ч. ошибка горизонта. В моей вылолженной проге вроде есть функции в класcе INS.
Я видел где-то на датчике от мыши, просто туда был прикручен объектив от камеры.
да, согласен, РРМ это 1 канал. Но откуда его взять в обычном приемнике? я так понимаю, приемник ловит РРМ и разделяет его на все 7 ножек в отдельности…
Ааа, так у вас обычный приемник…
Пришла тут идея, если свести все каналы в один, соединив их между собой после выхода из приемника?
Так как импулься между собой разделены, то они перекрываться не будут, тогда все 7 каналов можно пустить по 1 проводу и подцепить к одному каналу таймера.
ищем по “расстояние между точками на поверхности земли”, там мног всего
вот еще ссылочка www.movable-type.co.uk/scripts/latlong.html
Это подстава, “прорекламируй” поставщика.
dx 😦 я писал раньше что посылка потерялась, а потом ещё не мог найти их (датчики) чтоб показать, а оказывается их уже нет, вот они и переслали новую с датчиками движения на пирометрах 😦 я один сегодня на работе уже сломал 😃 ладно фиг с ними, невелика потеря, сейчас уныло смотрю в код AeroQad сколько всего переделать то надо - ууу, никак не насмелюсь пока даже порты переписывать, а надо ещё gps от арду впихнуть (он там есть но надо правильно библиотеки подключать научится), удержание высоты сделано чисто на баро 😦 сонара нет, короче пахать и пахать, а хочется сделать cli, дабы не компилить постоянно для изменения аппарата или там подключения модулей (gps, сонар…) ну и всяких там расширеных возможностей по настройке, а без мативатора (платы) как-то ничего не хочется делать…
dx 😦
Один раз связывался с dx, больше я туда ни ногой…