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

ИльяПРо

Есть мысли применить 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…(😃)

alexeykozin
SergDoc:

про “классную” квадратную шину

мораль: при планировании новых контроллеров надо отказываться от использования I2C в пользу SPI
благо у всех сенсоров включая HMC5983 и ms5611 она есть

ИльяПРо
rual:

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

Давайте, думаю не помешает, мне там только порты переобозначить по идее. Но теперь коптером займусь только в следующем году.

rual:

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

У меня тоже раньше вся обработка в прерываниях была, но дальше определенного предела так двигаться не получается, слишком нерационально процессорное время тратится. Работой FreeRTOS я доволен, все инструменты для масштабируемой и надежной системы есть (семафоры, мьютексы).

rual:

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

UKF полностью взял у Autoquad, слегка модифицировал, добавил компенсацию нецентрального расположения ЖПС, чутка улучшил коррекцию по ЖПС, и там по мелочи.
Линеаризуются все уравнения связанные с переходом из локальной СК в глобальную (они ж нелинейные или я ошибаюсь?), а также все операции связанные с кватернионами.

rual:

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

А можно поподробнее? просто выход курсового ПИДа ограничен?

oleg70:

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

В начале когда спутники ловит, да прыгает, но это около 10 секунд, потом она относительно четкая, график я приводил на предыдущей странице.
На первом видео на этом форуме, у меня высота определялась по акселю + коррекция от ЖПС через ПИ регулятор (по принципу Махони), и ничего четко летал даже в порывы ветра и резкие наклоны.

rual
SergDoc:

оказалось всё прозаично просто, баро тупил и не отпускал scl ( у слейва есть такая привилегия) и все - все хитромудрые алгоритмы коту под хвост!

СКЛ слейв удерживает если хочет захватить управление шиной. Серёг, зачем барику бунтовать на шине?

ИльяПРо:

Давайте, думаю не помешает, мне там только порты переобозначить по идее.

Завтра выложу.

ИльяПРо:

Но теперь коптером займусь только в следующем году.

Что так?

ИльяПРо:

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

То что масштабируется плохо это да, а насчет времени вряд ли.

ИльяПРо:

Линеаризуются все уравнения связанные с переходом из локальной СК в глобальную (они ж нелинейные или я ошибаюсь?), а также все операции связанные с кватернионами.

Кватернионы - линейная алгебра, всё должно быть линейно, кроме преобразования угловой скорости в ориентацию (там тригонометрия), но за период отчета в переделе макс угловой скорости ДУС синус линеен.

ИльяПРо:

А можно поподробнее? просто выход курсового ПИДа ограничен?

Да, и он весит в 2 раза меньше.

ИльяПРо
rual:

Что так?

Две работы, учеба

rual:

То что масштабируется плохо это да, а насчет времени вряд ли.

помню, что алгоритмы определения ориентации обсчитывались в прерывании SPI от MPU6500, от этого страдали прерывания по i2c, а когда понадобилось еще записывать буферы для логов, то пришлось разделять весь процесс на 2 части, плодить флаги, проверки. Может и стоило изобрести свой велосипед, но у этого решения больше минусов, чем использовать ОСРВ.

rual:

Кватернионы - линейная алгебра, всё должно быть линейно, кроме преобразования угловой скорости в ориентацию (там тригонометрия), но за период отчета в переделе макс угловой скорости ДУС синус линеен

Ну допустим есть вектор состояния {q0, q1, q2, q3, Vx, Vy, Vz}, где q0, q1, q2, q3 - кватернион ориентации, Vx, Vy, Vz - это проекции вектора скорости в глобальной СК. Допустим у нас есть датчик скорости, но он измеряет скорость в локальной СК vx, vy, vz. Вот с помощью каких матриц F(которая еще A) и B(при управлении) нам из кватерниона и вектора скорости локальной СК получить вектор скорости в глобальной СК? Нет таких матриц в данном контексте задачи, значит это уже нелинейная зависимость.
В принципе работа с кватернионами - не линейна, а скорее тригонометрическая, просто она красиво выражается без использования тригонометрических функций, но основание именно тригонометрия, что уже не линейно

SergDoc
rual:

Серёг, зачем барику бунтовать на шине?

а фиг его знает - дефект, отказался я от этой шины в мини совсем… скорей бы её запустить, а то уж больше года в “подвешенном” состоянии - нет времени домашние проблемы 😦 мини сейчас как винда 3.1.1 лежит на столе - ПО почти готово но не собираемо…

rual
ИльяПРо:

когда понадобилось еще записывать буферы для логов, то пришлось разделять весь процесс на 2 части, плодить флаги, проверки

Ну да, маневрирование задачами весьма удобно, но, повторюсь, время не экономит.

ИльяПРо:

Ну допустим есть вектор состояния {q0, q1, q2, q3, Vx, Vy, Vz}, где q0, q1, q2, q3 - кватернион ориентации, Vx, Vy, Vz - это проекции вектора скорости в глобальной СК. Допустим у нас есть датчик скорости, но он измеряет скорость в локальной СК vx, vy, vz. Вот с помощью каких матриц F(которая еще A) и B(при управлении) нам из кватерниона и вектора скорости локальной СК получить вектор скорости в глобальной СК? Нет таких матриц в данном контексте задачи, значит это уже нелинейная зависимость.

Насколько я понял, речь шла о преобразовании скорости в одной СК в скорость в другой СК? А то написано из глобальной в глобальную?
Если преобразование из одной в другую, то это линейное преобразование - умножение поворачивающего кватерниона на кватернион от вектора и на обратный вращающий кватернион (V’ = qvq–1). Все это линейные матричные преобразования.

Код приложил.

RusINS11F4.rar

ИльяПРо

Спасибо за код.

rual:

Насколько я понял, речь шла о преобразовании скорости в одной СК в скорость в другой СК? А то написано из глобальной в глобальную?

Поняли правильно, но у меня нет ошибки.

rual:

линейное преобразование - умножение поворачивающего кватерниона на кватернион от вектора и на обратный вращающий кватернион (V’ = qvq–1)

Я понимаю, но оно может и линейно в общем виде, в контексте задачи Xk = A*Xk-1 + B*U, это нелиненая зависимость и простыми матрицами A и B ее не решить. К тому же это не матричное умножение,а кватернионное.

2 months later
rual

Друзья, поздравляю всех с Наступающим! Всего самого наилучшего! Интересных проектов!

oleg70
rual:

Интересных проектов!

Что там у Вас с проектами ?
От себя поздравляю всех с праздником, мира всем, здоровья, и исполнения желаний в Новом году…

Samer

Всех с наступившим. Подскажите как сделать удержание по жпс.
Делать пид по растоянию ? Алгоритм куда рулить написал , вопрос в выработке уровня и продолжительности сигнала на наклон в нужную сторону. К примеру ушли на 1 метр , надо дать короткий импульс а если 10 метров. Когда будет подлетать к точке как затормозить чтоб не перелетел ?

alexeykozin

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

SergDoc

грубо: считываем ускорение с акселя, высчитываем скорость и путь, корректируем их более медленным GPS - рисуем пид…
можно пофилософствовать на эту тему - берём это всё упаковываем в фильтр новомодный, только со всеми приблудами типа вектора управления с обратной связью и пытаемся отказаться от ПИД-а, потом плюём на это дело и делаем комплиментарник+ПИД)))

ИльяПРо

Привет, с наступившим! Я тут свободный денек поймал и переписал один момент в UKF, у меня GPS стоит не в центре, а немного вынесено на 20 см примерно. Вот эти самые казалось бы 20 см по сравнению с погрешностью GPS 1-5 м не должны повлиять, но они очень сильно влияют, особенно при поворотах по курсу. Так вот до этого у меня все показания с GPS корректировались проекцией на центр коптера, а потом использовались в UKF, было не очень. Сейчас что то я внезапоно подумал, и решил, что это неправильно. Надо с показаниями датчиков ничего не делать, а исправлять модель датчика то есть само уравнение h(x). то есть теперь вынос GPS учитывается в модели датчика, а сами показания поступают в UKF непостредственно. И это был решающий момент, сразу убил несколько зайцев. Во-первых теперь не надо парится о том, что GPS не находится в центре вращения вместе с акселерометром и ДУСом. GPS можно хоть на отдельную штангу вынести, надо только посчитать расстояния по всем осям и занести в модель. То есть все повороты и маневры отрабатываются с учетом новой правильной модели. Во-вторых очень важное и крутое свойство - это то что для коррекции ориентации теперь не нужно движение, как раньше, коррекция ориентации происходит через показания GPS всегда, даже когда коптер стоит на земле или завис. И это свойство я нигде не видел, а оно оказалось очень полезным. Все стараются поставить GPS в центр, а оказывается вынести его подальше становится очень полезным. Я полагаю, что я первый кто догадался использовать вынос GPS для улучшения качества стабилизации. Раньше ориентация коптера зависела от GPS только когда есть движение и коррекция происходила когда коптер двигался. Сейчас коррекция происходит всегда и даже когда движения нет. Компас теперь почти не нужен.
Раньше при резких поворотах по курсу было слышно пятигерцовые стуки от тмоторов при коррекции по GPS и было небольшое уплывание по горизонтали. Также при зависании в одной точке спусти несколько минут коптер начинал гулять, так как ориентация не корректировалась без движения. Теперь коптер четко крутится вокруг своей оси и висит в точке, просто фантастика. Короче я рад.
Но есть скользкий момент - по идее я добавил еще одну зависимость между перемнными, тем самым снизив устойчивость, если раньше некоторые переменные могли позволить себе быть некорректными и при этом коптер вел себя нормально, то сейчас косяк с одной переменной убьет всю систему. Вопрос только на сколько сильно я снизил устойчивость и как часто эти косяки будут происходить и как UKF будет их обрабатывать.

SergDoc
ИльяПРо:

Я полагаю, что я первый кто догадался использовать вынос GPS для улучшения качества стабилизации.

Огорчу - джедаи…

ИльяПРо

А есть док-ва? просто если это казалось очевидным, и DJI давно юзают, то почему никто это больше не использует?