flybrain. передатчик + приемник + автопилот. powered by stm32
да в общем-то и описывать особо нечего - управляю я самолетиками (летающее крыло) такой штукой
А это причем?
не поясните и мне заодно, как-нибудь посермяжнее: на входе фильтра Калмана при перевороте получается разрыв - от 180 градусов до -180. Фильтр от этого просто дичает. Как “свернуть” шкалу и сделать непрерывной? Т
Вопрос -то в чем сопссно?
Между делом подключил компас. Еще более интересненько стало все работать. Исчез дрейф по yaw и ошибка после тряски почти совсем пропала. Вообщем с компасом конечно лучше, что и говорить, но уж больно зависим от разных внешних воздействий. Будем в дальнейшем как-то искать функцию компромиса между вкладом компаса и гироскопа в процесс корректировки.
Пробовал подносить мотор к плате. Конечно магниты мотора воздействуют на компас, начинаются искажения, но не фатальные. похоже мотор должен находится от платы на расстоянии не менее 10( а лучше 15) см. Тогда я искажений практически не наблюдаю. Если удастся размещать мотор и плату на 20 см, то вообще все почти хорошо.
Еще пробовал плату ставить с ноутом на вращающийся стул и крутил, эмитируя развороты самолета в полете. Горизонт стоит и не дрыгается. Вообщем, все даже лучше, чем я предполагал.
Начал пробовать детектор синхры в видеосигнале. Вроде теория сходится с практикой. Прерывания пошли по синхроимпульсам. За праздники попробую забацать горизонт на экране. Сделаю видео на показать.
Цитата Сообщение от AlexSneg Посмотреть сообщение Расскажешь как преобразовывал кватернион в углы и? Да
Расскажи принцип, я дальше сам попробую.
Видимо, варианта у меня два - или читать страшные слова, или оставить все как есть
Мы не можем тебе помочь до тех пор, пока ты не опишешь какие у тебя датчики, и что они измеряют. И какой процесс ты пытаешься моделировать. А умные слова придется в любом случае освоить, ибо “векторный матанализ”, сам понимаешь не есть тема средней школы. 😃
Расскажи принцип
q.x = sin(theta/2) * axis.x
q.y = sin(theta/2) * axis.y
q.z = sin(theta/2) * axis.z
q.w = cos(theta/2)
Между делом подключил компас.
А калиброффка?
Яв стремицца к истинному направлению оси X относительно Севера Земли?
Как стартует алгоритм? Яв с 0 ползет к истинному значению, или инициализация по вычисленному значению курса из показаний магнитометра?
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 от таймера, запускаемым от ССИ. Нужно только снаружи проца соеденить синхру СПИ и выход меандра от таймера. Работать будет так:
- проц, в обработчике прерывания об окончании ПДП, останавливает таймер и настраивает ПДП для вывода очероедной строки;
- после прихода ССИ с внешней ноги запускается таймер и дает такт на синхру СПИ.
То есть, цепочка “ССИ -> запуск вывода точек -> подгрузка очередной порции точек в СПИ” чисто аппаратная.
Я думал у вас изначально примерно такая схема закладывалась.