Создание собственной прошивки: Ардукоптер на контроллере АПМ

Поскольку 3Drobotics отказались от поддержки новых версий ардукоптера для контроллера АПМ - можно сказать что теперь на этом шикарном "железе", используя хорошие части оригинального кода и убирая откровенно лишний функционал можно подчистить, подправить и получить стабильную и функциональную прошивку. Зная особенности и сложные в настройке функциональные модули - исправить и переписать. В результате будет прошивка может быть и не столь напичканая возможностями как Pixhawk но стабильная и надежная.

Собственно первый шаг я уже сделал, вырезав из прошивки бинарные протоколы навигационных модулей и оставив универсальный NMEA освободил 20кб флеша (и есть еще ряд идей как отжать местечко. если оно понадобится). Это не означает что не будут поддерживаться модули юблокс или сирф. Все навигационные модули умеют работать в протоколе NMEA, нужно их будет только переключить.

Прежде всего объектом всех бед в версии 3.2 был признан INAV - инерциалка. ардукоптер весьма капризен к настройке компаса, если аппарат собран слегка криворуко и компас показывает с ошибкой градусов в 15 то вместо того чтобы идти к цели по дуге соответствующей этому углу ошибки коптер делает в небе скоростные петли известные как toilet bowl тоесть туалетные воронки. Совместно с моими коллегами по резульатам наблюдений за поведением коптеров сделали предположение, что причиной непредвиденного поведения в удержании позиции может быть инерциалка, поэтому первое то было сделано - возможность ее выключения. Но как показала практика при отключении инерциалки из за разброса точек получаемых от жпс в режиме удержания позиции наблюдаются рывки.

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

Спойлер: технические детали KPEMF

Навигационный фильтр.
Фильтр называется KPEMF , расшифровывается как Parallel Estimations Median Filter
состоит фильтр из системы прогнозирования массива вариантов местоположения текущей точки - (составляющих массив данных широты и долготы) и обычного но великолепного медианного фильтра.

медианный фильтр готовится из алгоритма сортировки
пример сортировки пузырьковым методом
http://www.tigoe.com/pcomp/code/arduinowiring/42/
void bubbleSort() {
int out, in, swapper;

for(out=0 ; out < numReadings; out++) { // outer loop
for(in=out; in<(numReadings-1); in++) { // inner loop
if( analogValues[in] > analogValues[in+1] ) { // out of order?
// swap them:
swapper = analogValues[in];
analogValues [in] = analogValues[in+1];
analogValues[in+1] = swapper;
}​

} }
}
и процедуры извлечения значения элемента оказавшегося в середине.
в случае нечетного числа элементов
median = analogValues[numReadings / 2];
или среднего арифметического серединных элементов если размер массива -четный.

но для работы этого фильтра нужен массив "параллельных" данных. Данные должны не запаздывать. В противном случае - фильтр будет запаздывающим.

Итак для приготовления параллельных данных нужен массив навигационных приемников. Штук 10 -15... мда.. это громоздко и дороговато...
пойдем другим путем... Создадим ряд данных прогноза позиции на основе предыдущих данных.
к примеру при наличии истории координат за последние несколько секунд и исходя из предположения что коптер в ходе движения имеет непрерывную кривую ускорений
при этом исходя из точки достоверного прогноза, позиции полученной исходя из округления двух последних сэмплов рассчитываем скорость характерную для этих данных и переносим проекцию перемещения относительно средней точки от среднего двух сэмплов (среднее создает задержку и требует коррекцию на S= Vt) с учетом полученной скорости
подобным образом вычисляются прогнозы исходя из 3,4,5,6 сэмплов
таким образом мы получаем массив "параллельных" данных из 6 прогнозов и 1 актуального значения GPS.
К этим данным применяется медианный фильтр,
после чего полученная медиана признается "достоверными данными" достоверные данные помещаются в массив и используются для нового цикла построения прогнозов
собственно реализованный на ардуино алгоритм, при подаче на вход данных навигационного приемника работает примерно так:




тоесть розовый тренд показывает "угловатые" сырые данные. желтым показан результат работы фильтра. Диаграмма указывает на то что фильтр обеспечивает сглаживание. причем при этом не происходит задержки.

скриншот работы двух АПМ от одного приемника с фильтром и без

(чуть позже уберу под спойлер)

Специфичные настройки фильтра kpmf.
в полном списке параметров
inav_tc_xy = 0 - отключение инерциалки по широте и долготе. не пытайтесь делать тоже самое на других прошивках, значение ноль применимо только к этой версии прошивки.
GPS_navfilterPE =9 число паралельных пронозов - размер массива. допустимый зиапазон значений 3-19 значение по умолчанию 9. чем больше число прогнозов тем сильнее фильтрация, но больше латентность к ускорениям системы.
GPS_navfilterSPD=5 число прогнозов при вычислении скорости. скорость используется для приращения расстояния при коррекции прогноза на время задержки. Диапазон 3-9 чем выше значение тем лучше фильтрация но хуже прогноз при ускорениях
Отключение KPEMF
отключение может понадобиться для сравнительного анализа, для просмотра сырых данных, поступающих с навигационного приемника.
для отключения фильтра: GPS_navfilterPE =0

Что мы имеем сейчас:
собрана прошивка под апм2, выполнены первые облеты в ходе которых были выявлены приоритетные задачи и интересные комбинации параметров настройки.

Спойлер: концепции развития

концепция высвобождения места во флеш памяти
1. делим прошивку на две субверсии, функционал:
тестовая

• терминал
• логирование всего
• автотюн

боевая

• сервогимбал (если вдруг кому надо)
• ретракты
• ограниченное логирование
• парашют
• прочее нужное

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


Новые фичи.
После получения стабильной версии дополним функционал наиболее интересными и не затратными с точки зрения расхода флешь памяти новинками от свежих версий ардукоптера 3.3 и выше
пока на заметке:a) PID scaling for battery voltage (disabled by default, see MOT_THST_BAT_ parameters)

ps. все описанное выше не значит что я забрасываю проект f4by.
все лучшее будет перенесено в наш 32 битный контроллер.
На Pixhawk переносить не будем.
по вопросу получения актуальной версии прошивки для участия в тестировании и обсуждении пишите на kozin@mail.ru

Актуальная версия прошивки: 015а

Актуальный, обновляемый в шапке файл параметров для тестирования:
https://www.dropbox.com/s/ii5b6rmep8i3j6e/tstparam.rar?dl=0
архив содержит файл параметров который можно загрузить через мишен планнер вкладка full parameters tree -> compare
и файл с описанием для чего требуется тот или иной параметр изменить

Спойлер: файл описания параметров продублирую тут:

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

WPNAV_ACCEL, 150 -допустимые ускорения 150 означает полтора метра в секунду за секунду. по умолчанию 100 что значит 1 метр в секунду за секунду


LOITER_LAT_IMAX,40 -максимальный размер накапливаемой ошибки от внешних факторов, при ошибке компаса большие значения способствует возникновению воронки
LOITER_LON_IMAX,40 -аналогичное по долготе по умолчанию 1000

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

INAV_GPS_XY_SPD, 5 -интесивность восстановления инерциальной скорости данными от жпс приемника, большие значения приводят к большему доверию жпс
INAV_GPS_XY_POS, 8 -интенсивность восстановления инерциальной позиции данными от жпс приемника, большие значения приводят к большему доверию жпс

INAV_GPS_DELAY, 3 - компенсация времени задержки выдачи координат жпс приемником в десятых долях секунды диапазон 1-5

INS_MPU6K_FILTER, 10 -встроенная фильтрация вибраций силами процессора ориентации, значение по умолчанию 0,
внимание! при значении 10 требуется перенастройка пидов стабилизации летательного аппарата. Если у вас нет возможности перенастроить пиды стабилизации оставьте этот параметр соотвествующим вашим пидам