Создание собственной системы стабилизации

ИльяПРо

Привет всем. Обкатал UKF на коптере, штука своебразная. Летает без использования магнитометра. Но!! если коптер висит на месте, курс уплывает и начинаются небольшие гуляния, в процессе гуляний, курс корректируется снова. Алгоритму важно чтобы центр отсчета акселя совпадал с центром датчика ЖПС, поэтому нужно либо лепить его на плату, либо придумывать программные ухищрения с компенсацией этого явления, как в моем случае. Из плюсов UKF - можно летать в POSHOLD на больших скоростях и довольно точно удерживаться в точке. То есть диапазон скоростей и наклонов большой. Из минусов - около 60-70 процентов процессорного времени кушает именно UKF. Очень сложен в настройке и капризен в показаниях датчиков.
Из наблюдений - я ошибался, что UKF по своей природе может отбраковывать неверные показания датчиков, на самом деле он может так делать, если разработчик внесет механизм выявления неверных показаний, при этом нужно просто увеличить ковариацию шума датчика, UKF сам перестанет вносить его в расчеты.
Снял пару видосов:

oleg70
ИльяПРо:

Снял пару видосов

Супер

jShadow
ИльяПРо:

Но!! если коптер висит на месте, курс уплывает и начинаются небольшие гуляния, в процессе гуляний, курс корректируется снова.

Я так понимаю, это в безветренную погоду. А если есть ветер, который вносит дрейф?

ИльяПРо

Без разницы в какую погоду, для данного UKF главное, чтобы было движение, тогда показания будут качественными. Ветер - это внешнее возмущение, и его отработка зависит от пидов удержания.

alexeykozin

на вид хорошо держится,
а где подробнее почитать про ваш софт и хард?
UKF - взяли от автоквада, остльное свое-арду-прочее? проц? сенсоры?

ИльяПРо

Хард - STM32f407VGT6 - оценочная плата Discovery, вообще в планах сделать порт по F4BY, не зря ж его покупал, но времени не хватает.
датчики - MPU6500, MS5611, HMC5883L, Ublox M8N.
Ну еще датчики тока на каждый движок - ACS712ELCTR-30A-T, общий ток - ACS756SCA-100B. Датчик напряжения - просто резистивный делитель. Это скорее так, для исследований.
Дополнительно стоит ардуина на один из каналов приемника, отключает и включает по щелчку тумблера пульта всю силовую часть на коптере, в качестве ключа - 3 P-канальных MOSFETа - вот эта штука меня много раз спасала.
В пульт через приемник Frsky шлется телеметрия (спутники, напряжение, состояние и тд).
Для логирования SD-карта по SPI работает через библиотеку FATfs.
Двиги - 750 KV, регули - HobbyWing 40 A. Частота PWM 490 Гц.
Вся плата стоит на резиновых демпферах.
По софту - всем заправляет FreeRTOS, приоритет ядра выше приоритета прерываний.
Вертятся основные две задачи - Control (где все пиды, микшер, конечный автомат) и UKF (один общий для ориентации и положения)
Также куча маленьких задач по обработке данных с датчиков, сбору буфера логов.
Система работает примерно так: есть некая задача запроса данных, либо по внешнему прерыванию, либо по таймеру - происходит запрос данных с датчиков. Спустя какое то время происходит прерывание соответсвующего интерфейса (SPI, I2C), в котором выдается семафор. Соответсвующая задача обработки ловит этот семафор и обрабывает данные, выдает семафор для логов или другой обработки. Потом все происходит заново. Следует все общие переменные отгараживать мьютексами, а общие с прерываниями переменные защищать критическими секциями.
С микшером есть некоторые проблемы - пока все в нормальном режиме - все ок,. НО если происходит насыщение по ЯВУ, то происходит конкуренция угловых пидов и пида высоты, из-за чего коптер теряет устойчивость. Смотрел код ARducopera, ничего не понял, больно наворочено, но наверное у них такой проблемы нет.
Для ДУСа стоит внутренний фильтр на 41 Гц, для акселя - 20 Гц. К слову все измеряют вибрации акселя, но это скорее косвенный показатель, потому как он не влияет на управление. Одним из паразитных свойств ДУСа явялеся восприимчивость к ускорениям, и чем меньше восприимчивость, тем качественее ДУС. У MPU ДУС воспринимает эти вибрации, из-за чего они прямиком попадают в ПИД, из-за чего сильно ухудшается устойчивоть, поэтому надо ставить датчик на демпфер, НО если иметь хороший ДУС, то плату можно жестко ставить на раму.
Еще одно замечание - на улице барометр гонит какую-то ерунду, а если есть порывы ветра, то полагаться на него вообще бессмысленно, можт это у меня такой барометтр. Естестно я его укрыл паралоном и защитил от солнца. Поэтому в основном полагаюсь на высоту по GPS - она довольно качественная.
Красный график - это высота используемая в ПИДе, белый - по барометру, зеленый - по ЖПС

alexeykozin

5611 баро бывают дефектные, попробуйте заменить,
в арду баро используется для долгосрочной, медленной коррекции высоты, кратковременый показатель - проекция акселей на нормаль
но скорее всего у вас так и сделано, иначе бы у вас не держал высоту т.к. данные жпс не настолько точны чтобы давать сантиметровую точность

ИльяПРо:

С микшером есть некоторые проблемы - пока все в нормальном режиме - все ок,. НО если происходит насыщение по ЯВУ, то происходит конкуренция угловых пидов и пида высоты, из-за чего коптер теряет устойчивость. Смотрел код ARducopera, ничего не понял, больно наворочено, но наверное у них такой проблемы нет.

в принципе если проблема только с превышением по яву - то тут просто можно лимитировать яв воздействие некоторой константой.
в современных арду микшерах есть приоритеты стабильность по горизонту- стабильность по высоте - стабильность по яву

тоесть если для обеспечения стабилизации по ролу и питчу не хватает диапазона пвм одного из моторов то на исправления проблем с явом отводится только разница между требуемым пвм мотора и максимально допустимым значением

к примеру калибровка регулей 1000-2000
пусть для стабилизации газ-рол-питч выходит на 1 мотор нужно 1900
для стабизации яв на 1 мотор требуется +200
но 2000 - 1900 = 100 это то что остается яву
уменьшать расчет ява только на 1 канал нельзя т.к. это отразится на стабильности по рол-питчу, поэтому лимитируют яв на все каналы пропорционально
немного упрощенная модель управления моторами в арду у трикоптера, там нет так называемого “стабилити патча” который усложняет чтение кода,
если интересно - гляньте

ИльяПРо

Спасибо! как раз такого объяснения не хватало, посмотрю на код с этой позиции.

oleg70
ИльяПРо:

в качестве ключа - 3 P-канальных MOSFETа

Нельзя ли поподробней про эту фишку. (схемку бы, или на словах…)

ИльяПРо:

Красный график - это высота используемая в ПИДе, белый - по барометру,

Да, барометр че то шумит не по детски, (может наводки какие…) , у меня реальный шум после фильтрации простым калманом сантиметров ~10 - 15…

ИльяПРо

шум барометра проявляется при резких наклонах, при активном маневрировании и сильных порывах ветра, если просто зависнуть, то барометр нормально показывает, не знаю как у людей что то на барометре летает, но при порывах ветра, показания скачут на 2-3 метра, даже с маленькой коррекцией от баро, коптер плохо летает. Может реально барометр битый.
вот схема откл/вкл силовой части

alexeykozin
ИльяПРо:

вот схема откл/вкл силовой части

посмотрите на микросхему btc555 хороший ключ, дешево и посто

ИльяПРо

Спасибо, но в продаже я его не нашел, кроме Чип и Дипа за 520 р. 3 IRF4905 обойдутся дешевле. К тому же, тут есть небольшая особенность, что надо коммутировать именно питание (+), так как если коммутировать (-), на управляющем выводе ардуины окажется все напряжение аккума. N - канал не оч подходит из-за неполного открытия, не хотелось проверять, поэтому сразу поставил P-канальный мосфет.

Gapey

если кому нужно , могу насыпать жменьку китайских BTS555 за шапку сухарей … 2 раза пытался купить у китайцев , и оба раза так и не смог заставить их работать … так и валяются до сих пор …
теперь покупаю только официально , с документами , и соответственно дорого …

oleg70
ИльяПРо:

Хард - STM32f407VGT6 - оценочная плата Discovery, вообще в планах сделать порт по F4BY, не зря ж его покупал, но времени не хватает.

Илья, а небыло каких либо мыслей применить RaspberryPi для сей задачи ? Я время от времени достаю свою с полки пробую разные идеи в этом направлении… и забрасываю обратно по разным причинам… Очередной “приступ” её использовать недавно был кстати… идея сама по себе заманчивая и перспективная…
(хотелось бы обменяться мнениями на этот счет…)

ИльяПРо

Есть мысли применить stm32f7 и дополнить UKF еще и данными с камер, предварительно обработанными ПЛИС, но это новая тема для меня, мб потом разберусь. Но в планах полюбому задействовать, что то помощнее Ф4. RaspberryPi у меня нет, я особо не вдавался в подробности работы с ней. А она может под ОСРВ работать? Я так понял проблема писать мозжечок коптера под мощным процом заключается в прямом доступе к перефферии и в поддержке ОСРВ. Или вы хотите мозжечок оставить на Ф4, а остальное, не требующее реального времени, отдать RaspberryPi?

SergDoc
ИльяПРо:

Есть мысли применить stm32f7

у меня пока лежит 746-й до зимы наверно 😦 до чего дошли руки - это залить u-boot линуксовый и потыркаться в консоль )))

oleg70
ИльяПРо:

Или вы хотите мозжечок оставить на Ф4, а остальное, не требующее реального времени, отдать RaspberryPi

Как вариант, да… (и видимо, он самый реальный). Основная мотивация - наличие мощного видеоядра (люблю я FPV,а OSD на LM1881 уже задолбало своими косяками), ну и конечно неоспоримые плюсы - “человеческий” звук, wi-fi, высокоуровневый питон… и т.д. и т.п.

SergDoc:

у меня пока лежит 746-й

А в чем фишка этого проца ? посмотрел, не понял,…

8 days later
rual
ИльяПРо:

Хард - STM32f407VGT6 - оценочная плата Discovery, вообще в планах сделать порт по F4BY, не зря ж его покупал, но времени не хватает.

Могу вывалить свои исходники низов Ф4БЫ, можешь оттуда надергать или взять как есть.

ИльяПРо:

Система работает примерно так: есть некая задача запроса данных, либо по внешнему прерыванию, либо по таймеру - происходит запрос данных с датчиков. Спустя какое то время происходит прерывание соответсвующего интерфейса (SPI, I2C), в котором выдается семафор. Соответсвующая задача обработки ловит этот семафор и обрабывает данные, выдает семафор для логов или другой обработки.

У меня все обработки в прерываниях.

ИльяПРо:

и UKF (один общий для ориентации и положения)

Сам написал? Что там лианеризируется?

ИльяПРо:

НО если происходит насыщение по ЯВУ, то происходит конкуренция угловых пидов и пида высоты, из-за чего коптер теряет устойчивость.

У меня микшер взвешивающий, т.е. курсовой шим в максимуме вывалит в канал моторв в 2 раза меньше.

SergDoc

Напишу как я про “классную” квадратную шину: приехала мне плата бракованая, пока чисто ось крутится вроде все хорошо как только запускаешь арду - ну тут как-то ещё пол беды, коннектится через УСБ через раз, но стоит откалибровать компас и перегрузить, мало того что настройки “слетают” ну на самом деле не “слетают” а практически недоступны, проц такое ощущение сбрасывает частоту. Менял я и компас и фрам и проц - ничего не дало, а оказалось всё прозаично просто, баро тупил и не отпускал scl ( у слейва есть такая привилегия) и все - все хитромудрые алгоритмы коту под хвост!

oleg70
ИльяПРо:

в основном полагаюсь на высоту по GPS - она довольно качественная.

Насколько качественная ? Я разные модули пытал (EB-500,EB-270,SIM68,SIRFIII), скачки по высоте в лучшем случае 10-15 метров, проявляются с частотой в несколько секунд…

SergDoc:

Менял я и компас и фрам и проц - ничего не дало

понимаю, бывает ())… я вот тоже недавно спалил свой “супер контроллер”, подав 12 вольт на вход АЦП, пришлось перепаивать (на ЛУТ плате) стоногий F407…(😃)