Создание собственной системы стабилизации
Во первых, не знаю как на STM, а на AVR если вошли в обработчик прерывания, то прерывания автоматом отключаются. Что бы их включить, надо либо завершить обработчик, либо включить руками.
Теперь, если вы не включили прерывания, то по выходу из обработчика, выполнится следующее прерывание которое стоит в очереди.
А вот если включили вручную прерывание посреди обработчика, то может выполнится любое другое прерывание, но по его завершению, работа продолжится. Тут кроется одна большая засада - если по каким либо причинам, у вас много раз произойдет наложение обработчиков, получится Stack Overflow.
Поэтому, прежде чем разрешать прерывания в теле какого либо обработчика, надо 5 раз подумать.
на стм одно прерывание может быть вызвано из другого согласно приоритету. чтобы стек не переполнялся, просто надо это учитывать. в раз 10 штук не придут никак.
извечный вопрос - что делать? вместо ифк датчиков прислали 4 пирометра, поругаться или с пирометрами что-нибудь замутить?
Интересное наблюдение. Подключил я к STM32F105 GPS модуль MTK3339. Мудохолся-мудохался, а модуль спутники не находит. И вот однажды в процессе отладки я остановился на точке останова и о чудо, модуль в помещении находи 6 спутником. После запуска программы, GPS буквально за 5сек потерял все спутники.
После запуска программы, 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 проводу и подцепить к одному каналу таймера.