flybrain. передатчик + приемник + автопилот. powered by stm32
q.w = cos(theta/2)
интересно, обязательно попробую на праздниках.
А калиброффка?
Ну вот я как сделал? Берем платку начинаем вращать по-всякому. Цель - выискать максимумы и минимумы на максимальной чувствительности (у меня +/- 1,3 гауса шкала). Тут у меня была самая главная засада в том, чтобы параметрами смещения внутреннего АЦП побаловАться. Как оказалось с нулевым смещением, у меня очень много шума лезет. Ну у меня оказалось в отрицательную сторону надо подвинуть. Оно более менее симметрично стало по осям в смысле параметра gain. Но ноль отъехал процентов на 30. Ну вот я нашел мин и макс значения по всем осям. Соответственно вычислил bias для каждой из осей. Затем взял из ДШ сколько оно гаусов на цифру дает. Умножил. Но в результате пришел к тому, чтобы нормировку вектора прямо сразу проводить. То есть реально я себе оставил только абсолютное направление вектора. Ну в EKF оно вошло гладко, как и заказывал.
Как стартует алгоритм?
Тут да, я себе этот вопрос тоже задал, как бы тут универсально извернуться. В конечно итоге ввел в фильтр матрицу 6Х6 с нормированным вектором гравитации и магнитного поля. И собственно проблема автоматом порешалась. Я теперь стартую алгоритм. Включаю режим установки горизонта. Держу плату горизонтально и даю команду запомнить референсное значение. Оно скидывает в матрицу два нормированных вектора. А дальше EKF подхватывает и тянет кватернион состояния платформы автоматически в референсное положение. Скидывать референс или корректировать можно даже во время полета будет. И даже наверно автоматом, когда видим, что ускорение отсутствует и тушка реально перпендикулярно вектору гравитации летит. Ну то есть, уже дело техники алгоритма стабилизации. Можно будет подумать как реализовать функцию оценивающую вес компаса и гироскопа на основании скорости GPS и бародатчика.
короче, я очень доволен результатом. И даже то, что график магнитометра все-таки не линейный по осям, но решающего значения это не имеет, так как я все равно тяну платформу в нулевое положение относительно референса. Вот оно лежит уже на столе 2 часа, но абсолютный дрейф по всем осям (самое главное по Yaw)нулевой на 100%. Чуть позже сегодня видео запишу как оно в конечном итоге работает.
И даже то, что график магнитометра все-таки не линейный по осям, но решающего значения это не имеет
Имеет.
Дело в том, что для акселя g всегда строго в центр земли, а вот значения по магнитометру когда он в горизонте присутствуют по всем трём осям, причем для разной местности они разные. В америке по одной из осей вообще минус.
Вот оно лежит уже на столе 2 часа, но абсолютный дрейф по всем осям
Яв может и не плывет, но угол показывает наверняка не соответствующий действительности. Найди 0 и крутись вокруг оси Z. 90 градусов будет показывать при реальном повороте где-то в 60
q.x = sin(theta/2) * axis.x
Ой, слушай ка, Это ж формулы преобразования вектора в q. Так ты в качестве состояния тела в математической модели системы используешь x,y,z или кватернион?
У меня ни x,y,z ни phi, psy, theta вообще не существует. Я из кватерниона не смогу никак xyz достать.
А ну въехал, ты типа имея гладкую theta из кватерниона обратно арксинус и арккосинус берешь. Так?
Что-то торможу сегодня, уже праздновать начали…😃
используешь x,y,z или кватернион
Кватернион
А ну въехал, ты типа имея гладкую theta из кватерниона обратно арксинус и арккосинус берешь. Так?
Ну да, в обратку.
Только арксинус не нужен. Фету из W доставай, считай sin(Фета), ищи x, y, z
Вот вектор и угол.
Про нормализацию не забывай.
Про нормализацию не забывай
Да, все. Я въехал в теорию. Спасибо. В принципе решение достаточно очевидно, надо было мне самому догадаться.
градусов будет показывать при реальном повороте где-то в 60
да, оно так и есть, и фиг с ним. Оно же в референсе помнит, что угол=60, видит ошибку по Z составляющей от этих 60, соответственно и тянет горизонт к этому углу. Я отключу гиру, аксель и оставлю только компас для теста. Сделаю видео как оно отслеживает положение по трем осям только на компасе.
Еще раз перечитал, понял о чем ты. Ты имеешь ввиду именно реальное направление на север относительно платформы? Ну, а кто мешает, например засечь его в момент снятия референсного положения плтаформы, а потом, как начнется полет, по GPS понять какое начальное смещение было? а так, в принципе да, фиг его знает, как пользователь плату расположил. Мне кажется тут без вспомогательных манипуляций пользователя перед стартом или в последствии GPS не получится.
Ты имеешь ввиду именно реальное направление на север относительно платформы?
Положение платформы относительно севера. Без этого навигацию для медленных и зависающих тел (коптер) не сделать. Мозг должен точно знать как расположено тело (курс) для того, чтобы вычислить в какую сторону пилить например домой или к очередной точке маршрута.
Для самиков это менее актуально, т.к. курс можно по гпс узнать.
Без этого навигацию для медленных и зависающих тел (коптер) не сделать.
Ну вот, а если так:
_ _ _
A = M*G;
_
где M - текущий мгновенный вектор на север
_
G - направление гравитации, текущее
* - векторное произведение
Затем:
_ _ _
N = A*G;
N - должно указывать на север уже относительно текущего, мгновенного горизонта платформы
екущий мгновенный вектор на север
Откуда он?
Только из компаса
Только из компаса, то, что ты в данный момент с компаса снимаешь. Ну ты боишься, что из за кривизны и нелинейности самого компаса будешь каждый раз разную ошибку на направлении севера иметь? Ну тогда только, если диаграмму компаса четко к шарообразному варианту приводить, как это в ДШ ST рекомендует. В принципе процедура у них описана, но больно уж геморная
Ну ты боишься, что из за кривизны и нелинейности самого компаса будешь каждый раз разную ошибку на направлении севера иметь?
Лична я не боюсь. Мой яв всегда показывает чотко. Главное хорошо откалибровать и локальное магнитное поле учесть.
Сейчас как раз удержание направления для квадрика доделал. Вечером испытаю.
Посмотри в любой открытый код любого автопилота. Конечный алгоритм стабилизации всегда опирается на углы Эйлера при оценке положения тела. Так что это не я придумал.
Дык о том и речь- что надо скопировать, а потом - самому подумать. 😃 Если “все так начали делать уже 3 года как” то это не значит что так надо делать, просто так проще…
А если начинать с первоисточников, то есть с с Шмыглевского с Бранцем- то такая мысль и в голову не придет… Изначально БИНС разрабатывались для стабилизации космических аппаратов…
Дринкер уже все объяснил…
Насчет вращения в канале крена при вертикальном положении Х. Дринкер прав, это не проблема, по крайней мере не проблема ИНС, это особенность отображения в углах Эйлера. У себя сменил знак атан2 при переходе в Эйлер вокруг Z, и всё - более не крутится, полная идентичность реальности. Предполагаю что это зависит от порядка вращений в программе-индикаторе, типа дуги крена и курса компенсируют друг друга, хотя имеют место быть. Но если удерживать поворот точно в плоскости XZ, то смена Эйлеров происходит скачком.
то смена Эйлеров происходит скачком
Есть там зона неопределенности Roll, с математикой спорить бесполезно.
Есть там зона неопределенности Roll, с математикой спорить бесполезно.
Я не спорю с математикой, я писал что дуги имеют место быть. Весь вопрос с величиной зоны неопределенности.
Весь вопрос с величиной зоны неопределенности
Алгоритм управления самолетом должен уметь работать внутри такой зоны. Вопрос в том как его этому научить? Каждый разработчик автопилота решает эту проблему индивидуально. Как это в некоторых алгоритмах происходит, я прекрасно вижу в их коде. 😃 А некоторые уже попробовали на практике.
Небольшой отчетец по проблеме OSD.
В принципе я научился делать развертку буфера памяти по весь экран используя аппаратный DMA через SPI порт. Сейчас осталось решить только одну проблему. В условиях многозадачности по прерываниям, когда около 5 задач одновременно хотят быть обработанными возникает задержка неопределенная в пределах 3 мкс на вход в процедуру обработки строчного импульса. Это создает эффект сдвига начала развертки. Если отключить все прерывания и оставить только видео развертку, то все ОК, на экране имеем четкий прямоугольник развертки. SPI запустил на 6Мгц, развертка будет 280 X 240 пикселов. По вертикали удобно получилось использовать одну и туже строку от четного и нечетного полукадра.
Сейчас осталось только решить проблему таймингов в условиях множественных прерываний. Жалко, что в STM не додумались сделать фичу аппаратного старта DMA канала по переполнению таймера. Тогда бы вообще можно было чисто аппаратно развертку организовать, но такого нет. Зато есть аппаратный старт таймера по изменению фронта на внешнем пине. В принципе этого должно быть достаточно, главное сделать правильный абсолютный замер от начала строчного импульса до софтового старта DMA. Это как бы теория. На практике буду бороться с этим делом на следующей неделе.
Ответ один, максимально высоко поднять приоретет прерывания видео. У кортекса прерывания прерываний )) очень жесткие по времени. Насколько я понимаю код в обработчике компактный и невариативный, если так, то на остальных обработчиках это не скажется.
У кортекса прерывания прерываний
Я знаю, и вроде выставил самый высокий на видео синхру. Но результат чего-то не очень. Буду еще дебагить этот момент, мог я накосячить с приоритетами, либо стандартная библа глючная. Запишу логи вызовов прерываний в риал тайме, проанализирую. Займусь этой темой завтра, сейчас рано выводы делать.
Проблема я так понию точно привязанный к ССИ программный запуск СПИ, сам СПИ в ведущем режиме? Тут можно посоветовать использовать СПИ в ведомом режиме, с внешним поточечным тактом на SCK от таймера, запускаемым от ССИ. Нужно только снаружи проца соеденить синхру СПИ и выход меандра от таймера. Работать будет так:
- проц, в обработчике прерывания об окончании ПДП, останавливает таймер и настраивает ПДП для вывода очероедной строки;
- после прихода ССИ с внешней ноги запускается таймер и дает такт на синхру СПИ.
То есть, цепочка “ССИ -> запуск вывода точек -> подгрузка очередной порции точек в СПИ” чисто аппаратная.
Я думал у вас изначально примерно такая схема закладывалась.
Тут можно посоветовать использовать СПИ в ведомом режиме,
Ну вот где ты раньше был, когда я плату разводил а? В то время я не думал, что может такая странная проблема вылезти. Мне внутри прерывания только DMA стартовать нужно, не более того. Ну пару проверок еще, чтобы начало кадра детектировать. Реально сейчас я вижу на экране сдвиг строк на 1,5 - 2 пиксела. И это явно из за множественности и вложенности прерываний. Большие сдвиги я вчера вычистил, а вот эту мелочь не могу никак. Главное я врубиться не могу, откуда такая задержка. Прерывание от синхры идет самым высоким приоритетом, и я вижу в дебаггере, что проц реально кладет на все текущие задачи и прерывания и стартует прерывание от синхры отрабатывать. Если выключаю все прерывания, а оставляю только видео синхру, то сдвига пикселов нет - на экране чистый, идеальный прямоугольник от развертки. (Кстати зазора при переходе от одного байта к другому нет в отличие от атмеги. То есть, имеем засветку 100% без разрыва как по вертикали так и по горизонтали.) Ну не должно оно так долго переходить на прерывания, что разница в 2 пиксела получается, не могу в это поверить. Надеюсь, что сидит где-то мой косяк.
Твою идею я обязательно попробую реализовать. Мысль очень здравая. Ща прикину как там таймеры каскадом засинхронизировать и какие пины у меня свободны по схеме. Никогда не пускал SPI в слэйве, сейчас доку открою. Мне кажется синхры там недостаточно, надо еще NSS сигнал дергать.
хм… Проект - амбициозен. Интересно наблюдать за тем, что сам, год назад, решал такие проблемы). Автор - не здавайся, и у тебя получится!
Ну вот где ты раньше был, когда я плату разводил а?
)) ну дык, я на твою ветку два дня назад наткнулся.
Мне внутри прерывания только DMA стартовать нужно, не более того.
www.gaw.ru/html.cgi/txt/doc/micros/…/2_4_5_2.htm
Вот в чем дело, прерывание прерывания занимает от 7 до 18 циклов, т.е. дрожание фазы в 11 циклов, при 166МГц это ±6.6е-8 сек, как раз примерно разбег в 2.5 точки. Это фича железная, в лоб её не обойти.
Если всё же есть желание удалять аппендицит через нижнее отверстие организма, то можно сделать так:
- на предыдущем ССИ пускаем таймер на длину строки;
- по перегрузу таймера запускается обработчик, в котором есть петля ожидания ССИ следующей строки;
- на конец строки ±6.6е-8 сек гарантировано будет вызвано твоё прерывание с ожиданием ССИ, и по приходу запустит ПДП.
В итоге к приходу ССИ проц готов и код неопределенной длинны выполнятся не будет, но платим мы за это пустыми циклами в петле. Кста, в петле нужно сделать отсечку по времени в случае не прихода ССИ, иначе вся система зависнет в случае сбоя видео.
ps: посмотрел внимательно описание, высокоприоритетное прерывание должно начать обслуживаться гарантированно за 12 циклов , эта хз … Возможно арбитраж на шинах с другими ПДП виноват…