Каналы X и Y расположены в плоскости кристалла, канал Z, соответственно, перпендикулярен данной плоскости.
Согласен с Вами, было бы логично, если бы канал Z вел себя неадекватно по сравнению с осями, которые лежат в плоскости кристалла. Но глючит именно канал Y. Питание, земля, опорное напряжение и тактирование общее для всех каналов.
MagicStart и MagicStop – это константы, признак начала и конца пакета, они равны ‘M’.
Version – это версия, в данном случае 20, DeviceType – схема коптера (2 = квадрокоптер по схеме +).
Извиняюсь, я ошибся в частоте опроса, и уже исправился в комментарии выше.
Упс, 500 значений с интервалом 0.01 сек. Это отнимает 5 сек перед каждым запуском двигателей :(
Что-то я сегодня слишком много ошибаюсь, мне даже как-то неудобно.
Нельзя ли в двух словах пояснить принцип?
Протокол предельно прост (но в старших версиях они его усложнили).
Все параметры телеметрии передаются по запросу одним блоком. Программа шлет с определенной периодичностью запрос на получение телеметрии (это просто один байт, символ ‘M’), в ответ устройство отравляет ей блок данных (полную информацию по телеметрии), который можно описать следующей структурой:
Далее программа визуализации парсит и визуализирует данные. Это актуально для версии Mw-WinGUI-1.04, в более старших версиях протокол другой.
Ели нужно, могу расписать более детально протокол и назначение полей структуры.
Извиняюсь, парни, я ввел вас в заблуждение. Решил все перепроверить, уж слишком красивые результаты получились как для интегрирования без внесения коррекции на данном датчике.
Посмотрел исходник программы визуализации, каналы телеметрии, которые я использовал для построения графиков при выводе, похоже, масштабируются в 8 раз:
gx = BitConverter.ToInt16(packet, o) / 8;
Поэтому, умножаем все результаты графиков на 8. Уход за 30 сек составляет 4 градуса. Завтра, на работе, перепроверю все на партактиве, возможно я смотрю не ту ветку в репозитории. Но 4 градуса больше похоже на правду.
Я для телеметрии повторил протокол телеметрии из MultiWii 2.0. Он очень прост в реализации и под него есть несколько программ для визуализации показаний телеметрии (изначально, я просто отбрасывал логи в UART, но по текстовым логам отлеживать динамические процессы тяжело, а писать свою программу для визуализации было лень). Я использую эту программу для визуализации.
Касательно калибровки, я описал алгоритм калибровки в комментарии выше. Выборка из 32 значений для усреднения очень мало, я использую 500.
Ну, не все так радостно. Я привел тесты для идеальных условий. Плюс, у меня канал Y плывет значительно сильнее остальных (и на этом канале проскакиваю одиночные выбросы с амплитудой в 9 разрядов). Подозреваю, что это проблема конкретно моего экземляра чипа (т. к. он у меня единственный, статистику собрать не могу).
Вот график для канала Y при тех же условиях. За 30 сек интеграл уплыл на четверть градуса.
Еще пару слов о калибровке. Калибровать нужно не раньше чем 5 сек после ресета датчика. И калибровать нужно «тщательно». Я считаю среднее арифметическое для выборки из 500 значений с интервалом 0.1 сек. На выборке из 300 значений результат получается хуже. Все расчеты я, пока, провожу в float, время для оптимизации еще не настало :)
Я понимаю, что комплементарный фильтр корректирует мгновенное значение угла с акселерометра при помощи интегрального значения с гироскопа.
В данном случае наоборот, интеграл с гироскопа (расчетный угол) корректируется данными с акселерометра.
По поводу zero rate level гироскопа что-нибудь посоветуете? Что у вас сыпется с выходов гироскопа в состоянии покоя?
Вот графики для LSM330DL, ось Х.
Желтый – сырые данные, град/с * 500
Зеленый – данные с учетом калибровки, град/с * 500
Красный – интеграл (угол), град * 100
Горизонтальный масштаб – одна клетка 5 сек.
Сразу после калибровки
Прошло около 3 мин, интеграл уполз приблизительно на 0.5 градуса
Самое веселое – нагрев (просто касаюсь пальцем корпуса микросхемы).
Что с этим можно сделать – ну, первое – калибровка, второе – ввод коррекции нуля от температуры (в моем случае не эффективно, я от этого отказался). Дрейф в любом случае остается (проявляется со временем). Полностью победить дрейф, ИМХО, невозможно. Единственный вариант корректировать интеграл по акселерометру.
Спасибо за многообещающий цикл статей. Давно присматриваюсь к STM8. Единственное что останавливает – отсутствие бесплатного компилятора для данного ядра.
Это будет уже скорость, а не ускорение и получить такой интеграл из данных с акселерометра не очистив их от проекции g не получится — будет шустро набегать неслабая погрешность.
Здесь я Вас не совсем понял. Пропустив данные с акселерометра через обычный ФНЧ, мы получим, скорее, «усредненное» значение кажущегося ускорения. Почему должна получится скорость, мы просто фильтруем данные с акселерометра, а не дифференцируем показания акселерометра?
Вы сами писали в тексте по ссылке, что по сути ПИД-регулятор — тот же комплементарный фильтр.
Они похожи, но не совсем. Дело в том, что комплементарный фильтр учитывает только текущую ошибку (рассогласование) между расчетным углом и углом, полученным с акселерометра. Он работает как пропорциональный регулятор, вносимая коррекция пропорциональна значению рассогласования. А вот ПИ-регулятор, учитывает интеграл ошибки (рассогласования). Вносимая коррекция пропорциональна значению текущей ошибки (П) и сумме предыдущих ошибок (И).
Может стоит перенести наше обсуждение в личку?
Ну, вроде, разговор по теме, может еще кому интересно будет.
Вибрации можно отрезать ФНЧ, если они не совсем низкочастотные.
Увы нет. Дело в том, что акселерометр не просто ловит вибрации (их действительно легко срезать фильтром). Дело в том, что вибрации существенно влияют на точность
акселерометра в целом.
Сталкивались ли Вы с этой проблемой?
Да, конечно. Это дрейф нуля. Он есть всегда, более того, он очень сильно зависит от температуры. Первое что можно сделать – это калибровка (по сути то, что Вы описали, но лучше не подбором а именно калибровать).
Каждый раз, перед запуском двигателей, пока коптер стоит на земле, делаем много замеров показаний гироскопа, вычисляем среднее арифметическое (по каждой оси) и принимаем этот результат за «нуль». В дальнейшем из показаний гироскопа вычитаем этот «нуль». Это уменьшаете дрейф, но он все равно есть. И при интегрировании быстро накапливается ошибка. Вот, что-бы от нее избавиться и приходиться вводить корректировку по акселерометру.
Более эффективно ИМХО, использовать ПИ-регулятор для корректировании (я вскользь описывал это здесь)
Ну, честно говоря, не знаю, что получится если использовать акселерометр одновременно и для коррекции расчетных углов и для определения собственного ускорения. Грубо говоря, можно попробовать вносить коррекцию из данных акселерометра пропущенных через ФНЧ, а для расчета собственного ускорения брать моментальное показание акселерометра, и, в свою очередь, учитывать это ускорение при вынесении коррекции. Нужно будет попробовать.
А что, если не секрет, Вы делаете? Тоже коптер или какой-то другой проект?
Пока погрешность, вносимая собственным ускорением через фильтр, меня не беспокоит. Я ее буквально не вижу, на фоне вибрации. В будущем, я планировал определять собственное ускорение через положение коптреа в пространстве и двойное дифференцирование. Как определить положение копптера – вопрос. Как вариант – GPS либо видео камера + распознавание объектов.
О, я рад, что кто-то в сообществе занимается подобной тематикой. Будем обмениваться опытом. Если что, не стесняйтесь, обращайтесь в личку.
Теперь по делу.
a=Ax-g*sinA
Идея понятна. У нас, в данном случае, разные подходы.
А как Вы планируете водить, в этом случае, коррекцию расчетного угла с гироскопа. Если не вводить коррекцию, то sinA будет накапливать ошибку при интегрировании. Предположим, что коптер стоит, датчики откалиброваны и т. д. В начальный момент времени Ax = g*sinA, а = 0; Ок. Но далее, Ax не меняется (коптер стоит на месте) в sinA начинает накапливаться ошибка. Это приведет к тому, что появится «несуществующее» собственное ускорение, которое будет расти по мере накопления ошибки интегрирования.
На данном этапе моя проблема — сильный дрейф гироскопа L3G4200D фирмы ST
Я начинал свои эксперименты с LSM330DL от ST. Помимо сильного дрейфа нуля гироскопа, у меня еще очень сильно шумел канал Y (относительно других каналов). Я перешел на MPU6050 – дрейф меньше, шумит тоже меньше.
Еще одна проблема, о которой я уже говорил – вибрации. Я, «на столе», получаю идеальные результаты, стоит поставить плату на коптер и включить двигатели – углы начинают плавать в диапазоне 2-3 градуса.
— отчего не учитывается влияние линейных ускорений при расчёте угла наклона
А как выделить линейные ускорения? Акселерометр дает «смешанное» ускорение (g+линейное ускорение). Если бы был датчик, который бы фиксировал только линейное ускорение – можно было вносить коррекцию по его показаниям.
Перемещения коптера будут вносить ошибку в работу фильтра.
Будут но не так существенно (собственное ускорение нашего ЛА незначительное и непродолжительное). На практике, намного большую проблему создает вибрация от двигателей, которую регистрирует акселерометр.
"… акселерометр дает нам проекцию вектора, который направлен вертикально вверх..."
В данном случае акселерометр измеряет «кажущееся» ускорение, по сути это вектор реакции опоры/подвеса и он направлен в противоположенную сторону от вектора силы тяжести. Хотя, направление данного вектора (верх/низ) не принципиально, мы можем расположить систему координат так, что вектор будет направлен вниз.
Согласен с Вами, было бы логично, если бы канал Z вел себя неадекватно по сравнению с осями, которые лежат в плоскости кристалла. Но глючит именно канал Y. Питание, земля, опорное напряжение и тактирование общее для всех каналов.
Version – это версия, в данном случае 20, DeviceType – схема коптера (2 = квадрокоптер по схеме +).
Остальные поля – собственно данные телеметрии.
Что-то я сегодня слишком много ошибаюсь, мне даже как-то неудобно.
Протокол предельно прост (но в старших версиях они его усложнили).
Все параметры телеметрии передаются по запросу одним блоком. Программа шлет с определенной периодичностью запрос на получение телеметрии (это просто один байт, символ ‘M’), в ответ устройство отравляет ей блок данных (полную информацию по телеметрии), который можно описать следующей структурой:
Далее программа визуализации парсит и визуализирует данные. Это актуально для версии Mw-WinGUI-1.04, в более старших версиях протокол другой.
Ели нужно, могу расписать более детально протокол и назначение полей структуры.
Вернее уход за 3мин составляет 4 градуса для канала X
Посмотрел исходник программы визуализации, каналы телеметрии, которые я использовал для построения графиков при выводе, похоже, масштабируются в 8 раз:
Поэтому, умножаем все результаты графиков на 8. Уход за 30 сек составляет 4 градуса. Завтра, на работе, перепроверю все на партактиве, возможно я смотрю не ту ветку в репозитории. Но 4 градуса больше похоже на правду.
Я для телеметрии повторил протокол телеметрии из MultiWii 2.0. Он очень прост в реализации и под него есть несколько программ для визуализации показаний телеметрии (изначально, я просто отбрасывал логи в UART, но по текстовым логам отлеживать динамические процессы тяжело, а писать свою программу для визуализации было лень). Я использую эту программу для визуализации.
Касательно калибровки, я описал алгоритм калибровки в комментарии выше. Выборка из 32 значений для усреднения очень мало, я использую 500.
Вот график для канала Y при тех же условиях. За 30 сек интеграл уплыл на четверть градуса.
Еще пару слов о калибровке. Калибровать нужно не раньше чем 5 сек после ресета датчика. И калибровать нужно «тщательно». Я считаю среднее арифметическое для выборки из 500 значений с интервалом 0.1 сек. На выборке из 300 значений результат получается хуже. Все расчеты я, пока, провожу в float, время для оптимизации еще не настало :)
Гироскоп в MPU6050 шумит меньше, и дрейф нуля меньше (особенно тепловой дрейф). Если интересно – могу попозже снять с него графики.
В данном случае наоборот, интеграл с гироскопа (расчетный угол) корректируется данными с акселерометра.
Вот графики для LSM330DL, ось Х.
Желтый – сырые данные, град/с * 500
Зеленый – данные с учетом калибровки, град/с * 500
Красный – интеграл (угол), град * 100
Горизонтальный масштаб – одна клетка 5 сек.
Сразу после калибровки
Прошло около 3 мин, интеграл уполз приблизительно на 0.5 градуса
Самое веселое – нагрев (просто касаюсь пальцем корпуса микросхемы).
Что с этим можно сделать – ну, первое – калибровка, второе – ввод коррекции нуля от температуры (в моем случае не эффективно, я от этого отказался). Дрейф в любом случае остается (проявляется со временем). Полностью победить дрейф, ИМХО, невозможно. Единственный вариант корректировать интеграл по акселерометру.
Здесь я Вас не совсем понял. Пропустив данные с акселерометра через обычный ФНЧ, мы получим, скорее, «усредненное» значение кажущегося ускорения. Почему должна получится скорость, мы просто фильтруем данные с акселерометра, а не дифференцируем показания акселерометра?
Они похожи, но не совсем. Дело в том, что комплементарный фильтр учитывает только текущую ошибку (рассогласование) между расчетным углом и углом, полученным с акселерометра. Он работает как пропорциональный регулятор, вносимая коррекция пропорциональна значению рассогласования. А вот ПИ-регулятор, учитывает интеграл ошибки (рассогласования). Вносимая коррекция пропорциональна значению текущей ошибки (П) и сумме предыдущих ошибок (И).
Ну, вроде, разговор по теме, может еще кому интересно будет.
Увы нет. Дело в том, что акселерометр не просто ловит вибрации (их действительно легко срезать фильтром). Дело в том, что вибрации существенно влияют на точность
акселерометра в целом.
Да, конечно. Это дрейф нуля. Он есть всегда, более того, он очень сильно зависит от температуры. Первое что можно сделать – это калибровка (по сути то, что Вы описали, но лучше не подбором а именно калибровать).
Каждый раз, перед запуском двигателей, пока коптер стоит на земле, делаем много замеров показаний гироскопа, вычисляем среднее арифметическое (по каждой оси) и принимаем этот результат за «нуль». В дальнейшем из показаний гироскопа вычитаем этот «нуль». Это уменьшаете дрейф, но он все равно есть. И при интегрировании быстро накапливается ошибка. Вот, что-бы от нее избавиться и приходиться вводить корректировку по акселерометру.
Более эффективно ИМХО, использовать ПИ-регулятор для корректировании (я вскользь описывал это здесь)
Ну, честно говоря, не знаю, что получится если использовать акселерометр одновременно и для коррекции расчетных углов и для определения собственного ускорения. Грубо говоря, можно попробовать вносить коррекцию из данных акселерометра пропущенных через ФНЧ, а для расчета собственного ускорения брать моментальное показание акселерометра, и, в свою очередь, учитывать это ускорение при вынесении коррекции. Нужно будет попробовать.
А что, если не секрет, Вы делаете? Тоже коптер или какой-то другой проект?
О, я рад, что кто-то в сообществе занимается подобной тематикой. Будем обмениваться опытом. Если что, не стесняйтесь, обращайтесь в личку.
Теперь по делу.
Идея понятна. У нас, в данном случае, разные подходы.
А как Вы планируете водить, в этом случае, коррекцию расчетного угла с гироскопа. Если не вводить коррекцию, то sinA будет накапливать ошибку при интегрировании. Предположим, что коптер стоит, датчики откалиброваны и т. д. В начальный момент времени Ax = g*sinA, а = 0; Ок. Но далее, Ax не меняется (коптер стоит на месте) в sinA начинает накапливаться ошибка. Это приведет к тому, что появится «несуществующее» собственное ускорение, которое будет расти по мере накопления ошибки интегрирования.
Я начинал свои эксперименты с LSM330DL от ST. Помимо сильного дрейфа нуля гироскопа, у меня еще очень сильно шумел канал Y (относительно других каналов). Я перешел на MPU6050 – дрейф меньше, шумит тоже меньше.
Еще одна проблема, о которой я уже говорил – вибрации. Я, «на столе», получаю идеальные результаты, стоит поставить плату на коптер и включить двигатели – углы начинают плавать в диапазоне 2-3 градуса.
А как выделить линейные ускорения? Акселерометр дает «смешанное» ускорение (g+линейное ускорение). Если бы был датчик, который бы фиксировал только линейное ускорение – можно было вносить коррекцию по его показаниям.
Будут но не так существенно (собственное ускорение нашего ЛА незначительное и непродолжительное). На практике, намного большую проблему создает вибрация от двигателей, которую регистрирует акселерометр.
В данном случае акселерометр измеряет «кажущееся» ускорение, по сути это вектор реакции опоры/подвеса и он направлен в противоположенную сторону от вектора силы тяжести. Хотя, направление данного вектора (верх/низ) не принципиально, мы можем расположить систему координат так, что вектор будет направлен вниз.