Автопилот на AT91SAM7 +17


1. Вступление


Радиоуправляемыми самолетами я увлекся еще в 2002 году, живя в Праге. Первый самолет был с размахом крыла 1.2 м, двигатель 2-х тактный ДВС. Вот фрагмент видеозаписи тех времен. Однажды в 2008 году мне в руки попал акселерометр ADXL202E. Имея опыт разработки устройств на контроллере семейства MCS-51(это был Рубидиевый стандарт частоты и система сбора данных), я решил сделать автопилот для радиоуправляемой модели Mentor. Первым делом я подключил акселерометр к контроллеру. Для простоты я выбрал отладочную плату с контроллером AT91SAM7, за удобство и простоту работы с ней. Надо заметить, что я решил не изучать ничего про автопилоты в интернете, а делать все самостоятельно с "0", чтобы было интересней.



Итак, волей случая в 2008 году мне довелось полетать на самолете и иметь возможность использовать на борту ноутбук и свою платку с акселерометром. Сегодня любой владелец
планшета или смартфона может такой эксперимент провести самостоятельно, поскольку
акселерометры имеются почти во всех устройствах. Вот тут-то я и сделал для себя первое "открытие", что для стабилизации тангажа акселерометр использовать можно (поскольку самолет не может слишком быстро набирать скорость или тормозиться), а вот для стабилизации крена он не годится. Поскольку при правильном развороте без скольжения боковые ускорения компенсируются креном самолета, и акселерометр их не чувствует. Вспоминается фильм Ёлки-2, где крен самолета отслеживали по стакану воды, что у настоящего пилота вызовет недоумение. Зачем тогда изобретать авиагоризонт?
Итак, в этих статьях я опишу то, что получилось у меня с 2008 по 2017 год, когда я в свободное время занимался этим проектом. В июне 2012 автопилот был 1 раз испытан реально. Тогда еще без GPS. В 2017 году было выполнено несколько испытаний уже с GPS. Одно из удачных, в замедленном режиме, но с не успевшим найти свое положение по GPS можно увидеть тут. Стартом является выключение пульта. Взлет полностью автоматически, потом попытка перехватить управление, когда стало понятно, что самолет летит не туда. Было выяснено, что GPS определил координаты в 1 км примерно от настоящего. Еще один успешный пролет по маршруту 17.10.2017. Съемка неудачная, но видно взлет и посадку. Посадка в поле по причине разряда аккумулятора. Однако, красивая. Весь полет полностью автоматический. Разработка началась с проработки программной модели автопилота с использованием Borland C++ Builder и симулятора полетов XPlane 6. В последствие программный код был адаптирован для контроллера почти без изменений. Автопилот ориентирован на простую стыковку со стандартными модулями радиоуправления. Как наиболее подходящий был выбран режим PPM (Pulse Position Modulation). Не со всеми современными пультами даже PPM на сегодня может работать: требуется чтобы последовательность импульсов PPM шла последовательно, а не начиналась одновременно как в некоторых современных пультах, таких как FlySky FS-16. Хотя решение для управления от таких пультов также присутствует в программном обеспечении, но на сегодня не испытано и нуждается в отладке. Система проверена на пультах Robbie Futaba FC-16 и Art-Tech EFLY-100B. Один из недостатков — необходимость вывода из недр приемника непосредственно сигнала PPM до декодера — распределителя импульсов на рулевые машинки.


2. Программно-аппаратная модель


Функциональная схема автопилота представлена на рис.1. Сердцем автопилота является блок LEVEL1. Этот модуль является высокоприоритетной функцией и выполняется независимо от других событий каждые (15.625 мс испытанный вариант) 46.875 мс. Его задача — удержание самолета в правильном полетном положении



Рис. 1
Входными данными для модуля является структура FSSi, которая содержит параметры крен (Roll), тангаж (Pitch) и воздушную скорость (Air Speed). Правильным положением считается при отсутствии сигналов Ref крен = 0 и тангаж = 0. Также этот модуль управляет контроллером двигателя самолета и обеспечивает удержание скорости, полученной от блока LEVEL2 в соответствие с полетным заданием. Модуль Level2 вносит возмущение в модуль LEVEL1 и таким образом заставляет его удерживать уже не нулевые значения крена и тангажа, а рассчитанные в соответствие с полетным заданием. Параметр скорости непосредственно передается из полетного задания. Выходом модуля является структура данных, которая содержит текущее положение элеронов(ailerons), руля высоты(elevator), руля направления или рысканья(rudder) и РУД (Ручка управления двигателем, Throttle). Эта структура в программе названа FSSo, Модуль имеет имя FltStSys (flight stabilization system). Она может быть подана в PWMC (Pulse Wide Modulation Controller) если активирован режим автопилота. В противном случае на PWMC подается структура, заполненная таймером в соответствие с измеренными значениями, полученными от приемника ВЧ. Это режим управления традиционный для радиоуправляемых моделей.
Модуль Level2 работает медленнее, 1 раз в секунду он формирует управляющие воздействия — смещает нулевые точки — для модуля Level1. Данные, передаваемые в Level1, помечены словом keep в программе. Т.е. это то, что должен удерживать модуль Level1. Этот модуль выполняет все остальные функции:
1) реализация взлетного режима.
2) Удержание высоты
3) Удержание курса
4) Расчет маршрута и проведение самолета по маршруту, заданному на SD карточке.
5) Запись полетных параметров на SD карту один раз в секунду. (Функция черного ящика)
6) При подключенном к одному из COM портов модуля bluetooth выдача телеметрической информации.
Основная идея построения LEVEL1 автопилота заключается в построении полностью инерциальной системы пилотирования самолета, не использующей внешние сигналы, такие как фотодатчики цветовой температуры неба и земли или например NAVSTAR или ГЛОНАСС для удержания самолета в нормальном полетном положении. При исчезновении сигнала GPS полет выполняется по магнитному компасу. Для обеспечения таких требований используются следующие датчики:


  1. Roll. Значение крена вычисляется по значению угловой скорости разворота самолета и его линейной скорости в предположении, что разворот происходит без скольжения. Аналоговый сигнал от датчика угловой скорости GYRO оцифровывается встроенным в контроллер АЦП(ADC1) и далее модуль Calc вычисляет крен.
  2. Pitch. Значение тангажа (pitch) контроллер получает путем измерения таймером длительности импульса от датчика-акселерометра. Это позволительно делать, поскольку как правило слишком быстрого изменения скорости (кроме взлетного режима) у самолетов обычно не бывает.
  3. Altitude. Для измерения высоты используется датчик абсолютного давления. Данные получаем в виде напряжения, которое оцифровывается одним из каналов АЦП(ADC2) контроллера.
  4. Course. Для получения значения курса используется либо GPS модуль, либо, при его недоступности или при нулевой скорости (перед стартом контроль курса), цифровой компас Honeywell c датчиками Холла. Подключение к контроллеру по интерфейсу I2C.
  5. Speed. Датчик воздушной скорости на трубке Пито. Используется датчик относительного давления. Напряжение с датчика измеряется одним из каналов АЦП(ADC3) контроллера. Также для некоторых вещей используется значение скорости с модуля GPS.
  6. GPS. Модуль GPS подключен через com порт. Служит для получения маршрутных данных и скорости относительно земли (GND speed).
  7. SD card. Не является датчиком, однако с нее считываются данные о настройках модулей (коэффициенты для формул), данные о маршруте. На нее записываются полетные данные раз в секунду.

3. Датчики и модули


3.1 Акселерометр


Датчик предназначен для измерения линейного ускорения. В качестве акселерометра применен прибор ADXL202E (Analog Devices). Это двухосевой акселерометр с выходом в виде импульсов, длительность которых пропорциональна ускорению. Измерение длительности импульса осуществляется таймером. Только одна из осей (X) используется для измерения наклона самолета. Ось Y можно использовать для вычисления поперечного ускорения и его компенсации путем триммирования положения руля направления. Сейчас это не реализовано. Этот датчик показывает положение "шарика" в соответствующем датчике угловых скоростей и скольжения самолета (Например, Я-18Т, ЭУП-53). При развороте (под разворотом у пилотов понимается любое изменение курса самолета) пилот для уменьшения скольжения обычно подруливает педалями и удерживает шарик в центральном положении. Дрейф акселерометра при измерении наклона представлен на рис. 2. Как видно при помещении в холодильную камеру за 15 мин дрейф составил -5 градусов.



Рисунок 2


3.2 Гироскоп


Ключевым элементом панели любого самолета является прибор авиагоризонт. Он обычно строится на базе механического гироскопа. Он показывает крен и тангаж самолета в режиме полета по приборам. В данном проекте используется полупроводниковый гироскоп MEV-50A для измерения крена самолета. Однако, в отличие от классического гироскопа полупроводниковые гироскопы измеряют не абсолютный крен, а его производную — угловую скорость. Именно это является главной причиной, почему нельзя использовать полупроводниковый гироскоп для измерения крена самолета. Дрейф подобной системы измерения ничем не определен при небольших угловых скоростях. Если, например, в шлеме VR (виртуальной реальности), в котором гироскоп выполнен не на основе магнитного компаса с датчиками Холла, а на полупроводниковом приборе, попробуете очень медленно поворачивать голову, вы обнаружите, что он "не замечает" поворота головы. Поэтому я применил другой способ вычисления крена. Гироскоп устанавливается для измерения скорости разворота ? самолета вокруг вертикальной оси. Тогда крен самолета можно вычислить по формуле ? = arctg(V^2/Rg) = arctg(V?/g). См. рис.3.



Здесь R — радиус разворота самолета, g — ускорение свободного падения (9.8 м/с2), V -скорость относительно земли (истинная воздушная скорость), ? — угловая скорость, рад/с. Истинную скорость выдает GPS, или ее необходимо вычислять исходя из курса, скорости и направления ветра. В данном проекте используется скорость по GPS. Для исключения умножения на 0, минимальная скорость для расчета ограничена снизу 10 км/ч. К гироскопу, установленному таким образом, предъявляются очень серьезные требования по точности измерения. Так, первый гироскоп, который был опробован привел к потере самолета. Видеозапись полета можно посмотреть тут, данные "черного ящика" также доступны. Самолет бы поднят с пульта на высоту около 150м и был включен автопилот. Однако, как видно из графиков, с самого начала гироскоп давал неверные данные — Gyr на картинке — и, как только на 97 секунде полета был включен автопилот, самолет ожидаемо вошел в штопор. К сожалению, вывести его благополучно не удалось из-за ошибки эргономики: ручное управление включалось с пульта при среднем положении тумблера, которое вовремя от волнения найти не удалось:). Это был гироскоп от простого соосного вертолетика. Вообще, мне было понятно с самого начала, что так и будет, поскольку температурные испытания в холодильнике показали огромный дрейф этого прибора. Уход за 15 минут составил около 15гр./c, хотя нормальные рабочие значения этого параметра обычно не превышают нескольких градусов в секунду во всем диапазоне кренов (до 30 гр.). А точность должна быть не хуже 0.1o/c.
Посмотреть на данные тестирования и на процесс измерения можно здесь. Сравнение характеристик гироскопов представлено на рис.5 Можно видеть, что в общем и нельзя было ожидать от noname гироскопа нормального полета, если его дрейф за 15 мин. в морозильной камере составил примерно 24 °/с. Сейчас используется гироскоп MEV-50A. Видно, что его дрейф за 25 минут порядка 0.6 °/с.




Рис.5


3.3 Высотомер


В качестве высотомера обычно используют датчик абсолютного давления. В данном проекте применен датчик MPXA4115A6U. Датчик выдает напряжение, которое измеряется контроллером. Перед полетом его необходимо прогревать. Время прогрева достигает 10-15 мин. Поскольку атмосферное давления меняется, перед каждым полетом высотомер калибруется автоматически. Датчик обладает значительным температурным дрейфом. На рисунке 6 представлен график дрейфа высотомера. Поскольку датчик скорости так же является измерителем давления то дрейф у него такой же.



Рис. 6
Наиболее интересным испытанием датчика давления является несомненно поездка в метро. Данные черного ящика поездки показаны на рисунке 7. Про этот график можно сказать, что автопилот включен на 12 этаже, (около 36 м) Далее спуск на лифте, потом я ехал на автобусе, и потом на метро. Конечная точка моего путешествия, когда я вышел из метро, действительно на возвышенности.



Рис.7


3.4 Датчик воздушной скорости.


Воздушную скорость обычно измеряют с помощью трубки Пито. В принципе эти вещи обычно рассчитываются. Но понятно, что если подуть снаружи в трубку (не касаясь ее губами), то давление в ней будет повышаться. Измеряя давление в трубке можно оценить скорость полета. Трубку обычно размещают либо на крыле, либо на фюзеляже (если нет тянущего винта). Для правильного измерения необходимо, чтобы двигатель не нагнетал воздух непосредственно в трубку. Будучи в этом деле "первопроходцем", я применил датчик относительного давления MPXV5004GP. Также как и у датчика абсолютного давления, выход этого датчика напряжение, его измеряет контроллер. При запуске он также калибруется. Но не требуется грубая подстройка с помощью резистора, если не удается обнулить высоту в отличие от датчика абсолютного давления. Трубка была взята из углеволокна диаметром 3 мм. См. рис. 8.



Рис.8
Для калибровки датчика трубка была установлена на зеркало заднего вида автомобиля, и был произведен контрольный заезд, текущая скорость фиксировалась на диктофон. Если совместить аудио и лог можно откалибровать датчик воздушной скорости. Так я "открыл" для себя, что скорость пропорциональна корню квадратному от давления. V = k*vp В дальнейшем правильность измерения скорости подтвердилась при испытательных полетах с GPS. Температурный дрейф датчика воздушной скорости можно увидеть на рис. 6. Только что совершено научное открытие. При разгоне воздух откачивается из салона автомобиля, давление падает, что соответствует росту высоты? Так ли это? Корреляция с высотой не может быть случайна…


3.5 Цифровой компас


Цифровой компас позволяет определять направление полета при отсутствии сигнала или приемника GPS. Изначально система строилась как инерциальная. Программа полета записывалась не в виде координат GPS, а задавалось направление, скорость и время полета до следующей точки. В качестве компаса был применен 2-х осевой компас HMC6352. Если бы тогда разработчик был умнее он бы применил конечно 3-х осевой компас. Но сделано пока так. Компас имеет интерфейс I2C. Этот прибор может выдавать непосредственно курс, либо уровни составляющих магнитного поля. Для увеличения точности необходимо иногда, или при изменении внешней магнитной обстановки производить его калибровку. Для калибровки необходимо при включении питания автопилота удерживать кнопку калибровки, поле чего контроллер на 1 минуту входит в режим калибровки. Режим калибровки индицируется особым миганием индикатора режима работы (J9, BLUE-LED, 4 раза в секунду). В это время необходимо в строго горизонтальном положении самолета произвести несколько полных разворотов самолета. Для того чтобы можно было пользоваться датчиком, для корпуса автопилота нельзя применять магнитные материалы. К сожалению, первые же эксперименты показали, что точность измерения хороша только в случае строго горизонтального положения прибора. На самолете это требование выполнить невозможно. Причем точность не просто падает, а ее нет вообще. При наклоне даже на небольшой угол, несколько градусов получаем на выходе все +-180°. Поэтому пришлось отказаться от получения непосредственно курса от компаса, а получать значения компонентов магнитного поля, и вычислять курс применяя насколько это возможно компенсацию. За неимением третьей оси Z пришлось принять значение этой компоненты константой. Результат работы компаса с компенсацией и без показан на рис.9. HDGn — некомпенсированный компас, HDGc — компенсированный. Как видно из графиков, компенсация значительно улучшает качество измерений курса, однако о точности в десятые доли градуса речи конечно не идет. Также работу компаса можно увидеть на полетных данных без GPS 11.09.2012. Здесь полет совершался в Вохоново. Нужно заметить, что в полетах в 2009 г. еще не было компенсации компаса. Как видно на рисунке старт действительно происходит с курсом близким к 120 градусам, и завершается в лесу с курсом около 300 градусов. Как показали поздние полеты, сильное раскачивание самолета происходило (да, да!:)) из-за плохого крепления крыла к фюзеляжу и снижения скорости полета после выключения двигателя.



Рис. 9.


3.6 Модуль GPS


Применен GPS модуль с встроенной антенной. Это небольшая платка, располагающаяся снаружи экранированной коробки автопилота. (См. Рис.8). Документацию на модуль можно найти в папке GPS. Применен модуль S3126. GPS модуль подключен к контроллеру через один из UART. Общение с модулем идет стандартным протоколом NMEA0183. На испытании 2017_09_02 перед запуском самолета GPS сработал некорректно, и самолет очень красиво полетел не туда. Как можно было видеть из данных "черного ящика" TRACK005.LOG GPS "думал" что он в километре от заданной точки. Это можно посмотреть в полетных данных от 2017_09_02. Надо заметить, что в этом испытании я еще не мог контролировать расстояние до следующей полетной точки, пока был включен пульт (т.е. выключен автопилот). Поэтому расстояние 134 М в лог файле не говорило о том, что место самолета определено корректно. Расстояние до следующей точки должно было быть около 100 м, смещение около 0, а курс 323. Однако, как видно реальные данные появились только в момент взлета в 14:10:33. И курс оказался 150, расстояние 1012М, смещение 121М (отклонение от оси, по которой должен лететь самолет в соответствие с заданием). После этого испытания в программу было внесено изменение, чтобы маршрутные данные отображались в телеметрии независимо от режима. Стало возможным видеть перед запуском, корректно ли работает GPS. Этот контроль также отражен в карте запуска. Автопилот получает от GPS модуля следующие параметры: время, дату (используется при записи лог файла), готовность, количество видимых спутников, координаты долготу, широту и скорость в узлах(knots). Готовность GPS отображается светодиодом зеленого свечения, мигающего в моменты записи информации на карту, примерно 1 раз в секунду. Важно понимать, что GPS модуль не может так же точно, как координаты определять высоту, поэтому высота не используется. При включении автопилота, сразу после прогрева и калибровки датчиков, контроллер производит расчет маршрута по координатам полетного задания. Этот маршрут не записывается на карту, но выводится через BT телеметрию. По этим данным можно оценить правильность полетных данных. Программа сейчас не ждет инициализации GPS, однако это легко изменить. Вот так выглядит выдаваемый лог телеметрии:
GPILOT V2.17.0 24/Jul/2017
Waiting GPS…
GPS ready…
Init from MMC card..Hidden sectors = 57
G0,60.674983,29.173461,0,0,0,0,1
G1,60.675504,29.172667,30M,50kM/h,323D,5s,2,72M
G2,60.676388,29.171305,30M,50kM/h,322D,8s,3,123M
G3,60.675641,29.168697,30M,50kM/h,239D,11s,4,164M
G4,60.672877,29.172249,30M,50kM/h,147D,26s,5,362M
G5,60.673546,29.174448,30M,50kM/h,58D,10s,6,140M
G6,60.674767,29.173783,20M,30kM/h,345D,16s,7,140M
G7,60.675737,29.172313,20M,0kM/h,323D,0s,0,134M
Compass calibrate skip…
Sensors calibrate…
Sensors calibrate complete…


3.7 Модуль Bluetooth


Для удобства контроля параметров перед взлетом и вообще отладки, выход через UART был оснащен модулем bluetooth Parani-ESD1000. Для связывания с другими устройствами установлена кнопка. Наиболее удобным приложением для Android устройств оказалось приложение Bluetooth Graphics. Доступно в Google Play.


3.8 SD — карта


Как любой уважающий себя самолет, мой проект имеет на борту "черный ящик", роль которого играет SD карта. Карта подключена по медленному SPI интерфейсу, для ее подключения я взял готовый код, от Rolf Freitag, за что ему благодарен. Код обеспечивает чтение и запись блоков по 512 байт (Сектор). На карту записываются параметры настройки автопилота, маршрут полета. При работе на карту один раз в секунду пишутся данные, которые программист пожелает записывать. Когда я начинал работу над автопилотом, у меня в распоряжении оказалась SD карта размером 16 МБ, поэтому сначала была написана FAT12, и только этим летом я дописал FAT32. Для ее использования нужно файл fat.c и fat.h в проекте заменить на fat32.c и fat32.h. По мере износа SD карты как показала практика, количество скрытых секторов может увеличиваться. Это связано с износом SD карты и с тем что начальные сектора перезаписываются чаще других, особенно в системе FAT12. В системе FAT32 таблица размещается в произвольных местах карты как фрагментированный файл, поэтому износ идет медленнее. Полетные данные записываются в лог файл вида TRACKXXX.LOG автоматически. Данные для записи пишутся на карту сразу, без буферирования, один раз в секунду, причем производиться обновление сразу же и файловой таблицы, для того чтобы можно было иметь полетные данные вплоть до последней секунды полета. Благодаря GPS каждый лог-файл имеет актуальную дату и время создания. В модуле GPS введена коррекция на +3 часа для корректного отображения времени часового пояса Санкт-Петербург.




К сожалению, не доступен сервер mySQL