Python Raspberry Pi Pixhawk и квадрокоптер. Или как не надо делать роботов

Основной процессор

8051 vs AVR vs PIC vs ARM: Семейство микроконтроллеров составляющее основу большинства современных контроллеров полёта. Arduino основан на AVR (ATmel), и сообщество, похоже, сосредоточено на MultiWii, как на предпочтительном коде. Microchip является основным производителем чипов PIC. Трудно утверждать, что одно лучше другого, всё сводится к тому, что может делать программное обеспечение. ARM (например, STM32) использует 16/32-битную архитектуру, при этом десятки используют 8/16-битные AVR и PIC. Поскольку одноплатные компьютеры становятся все менее и менее дорогостоящими, ожидается появление полётных контроллеров нового поколения, которые могут работать с полноценными операционными системами, такими как Linux, или Android.

ЦП: Обычно их разрядность кратна 8 (8-бит, 16-бит, 32-бит, 64-бит), что в свою очередь указывает на размер первичных регистров в ЦП. Микропроцессоры могут обрабатывать только установленное (максимальное) количество бит в памяти за один раз (такт). Чем больше бит может обработать микропроцессор, тем более точной (и более быстрой) будет обработка. Например, обработка 16-битной переменной на 8-битном процессоре происходит куда медленней, чем на 32-битном. Обратите внимание, что код также должен работать с правильным количеством бит, а на момент написания этой статьи лишь немногие программы используют код, оптимизированный для 32 бит.

Рабочая частота: Частота, на которой работает основной процессор. Также по умолчанию её называют «тактовой частотой». Частота измеряется в герцах (циклов в секунду). Чем выше рабочая частота, тем быстрее процессор может обрабатывать данные.

Программная память/Флэш: Флэш-память — это место, где хранится основной код. Если программа сложная, она может занимать много места. Очевидно, что чем больше память, тем больше информации она может хранить. Память также актуальна при хранении данных в полёте, таких как координаты GPS, планы полёта, автоматическое движение камеры и т.д. Код, загруженный на флэш-память, остается на чипе даже после отключения питания.

SRAM: SRAM расшифровывается как «Статическая память с произвольным доступом» и представляет собой пространство на чипе, которое задействуется при выполнении расчетов. Данные, хранящиеся в оперативной памяти, теряются при отключении питания. Чем выше объём оперативной памяти, тем больше информации будет «легко доступно» для расчетов в любой момент времени.

EEPROM: электрически стираемое программируемое постоянное запоминающее устройство (ЭСППЗУ) обычно используется для хранения информации, которая не изменяется во время полёта, например настройки, в отличие от данных, хранящихся на SRAM, к которым могут относиться показания датчика и т.д.

Дополнительные порты Ввода/Вывода: большинство микроконтроллеров имеют большое количество цифровых и аналоговых портов ввода и вывода, на контроллере полёта некоторые используются под датчики, другие для связи, либо для общего ввода и вывода. К этим дополнительным портам могут быть подключены RC сервоприводы, системы подвеса, зуммеры и многое другое.

Аналого-цифровой преобразователь (A/D converter/АЦП): Если датчики используют бортовое аналоговое напряжение (обычно 0-3.3В или 0-5В), аналого-цифровой преобразователь должен преобразовать эти показания в цифровые данные. Как и в случае с процессором, количество бит, которое может быть обработано АЦП, предопределяет максимальную точность. С этим связана тактовая частота, с которой микропроцессор может считывать данные (количество раз в секунду), чтобы убедиться, что информация не потеряна. Тем не менее, трудно не потерять часть данных во время такого преобразования, поэтому чем выше разрядность АЦП, тем более точными будут показания, но при этом важно, чтобы процессор смог справиться с той скоростью, с которой отправляются данные.

Выбор гироскопов: что лучше высокая частота опроса или шум?

У IMU есть две основные характеристики: максимальная частота сэмплирования и насколько полученные данные будут зашумлены (механическими вибрациями и электрическими помехами).

В настоящее время очень часто используют микросхему MPU6000, которая поддерживает частоту опроса до 8k, и обладает (неоднократно проверено) хорошей устойчивостью к разного рода шумам и помехам. Главное стараться избегать MPU6500 и MPU9250, хотя у них больше рабочая частота, но и уровень шумов тоже значительно выше.

Учтите, что разные серии гироскопов ICM имеют разные характеристики. ICM20689 — один из худших вариантов, легко восприимчив к шуму, да и с надежностью проблемы. Если приходится выбирать из ICM, то берите модель 20602.

В последнее время появляется всё больше и больше ПК с гироскопами на отдельной плате с антивибрационной развязкой (кусок поролона, чтобы снизить вибрации от моторов).

Обновление (окт 2023). Начиная с версии Betaflight 4.1 нет поддержки частоты 32кГц, так что если вы используете гироскопы ICM с Betaflight, то looptime будет не больше 8кГц.

Скорость работы гироскопов — это палка о двух концах: если питание чистое, и шумов нет, тогда серия ICM на 32k будет работать лучше, чем MPU6000. Однако, если регуляторы и моторы начнут генерировать помехи, а коптер вибрирует, тогда ICM хуже, чем MPU6000.

Несколько советов как крепить ПК с демпферами (антивибрационное крепление) и использовать конденсаторы для фильтрации помех по питанию.

Mavproxy

MAVProxy уже установлен в образе Navio. Его также можно

и на ПК (Windows, Linux, MacOS) для дальнейшего общения с автопилотом в консольном режиме.

Убедившись, что Ardupilot работает, запустим на Raspberry скрипт MAVProxy такой командой:

mavproxy.py --master=udp:127.0.0.1:14550

Параметр

–master=udp:127.0.0.1:14550

задает для скрипта источник данных. Это локальный UDP-порт, который был прописан в файле конфигурации Ardupilot. После запуска команды, MAVProxy соединиться с этим портом и выведет на экран сообщения автопилота, примерно как у меня:

pi@navio:~ $ mavproxy.py --master=udp:127.0.0.1:14550
Connect udp:127.0.0.1:14550 source_system=255
Failed to load module: No module named adsb. Use 'set moddebug 3' in the MAVProxy console to enable traceback
Log Directory: 
Telemetry log: mav.tlog
Waiting for heartbeat from 127.0.0.1:14550
 MAV> online system 1
STABILIZE> Mode STABILIZE
fence breach
GPS lock at 0 meters
APM: APM:Copter V3.5.5 (88a1ecdd)
APM: Frame: UNKNOWN
APM: PreArm: RC Roll not configured
APM: PreArm: Compass not calibrated
APM: PreArm: 3D Accel calibration needed
APM: PreArm: check firmware or FRAME_CLASS
APM: PreArm: Throttle below Failsafe

Так как автопилот еще не откалиброван и до конца не настроен, то об этом красноречиво говорят и сообщения. В этом режиме можно общаться с автопилотом посредством команд. Если бы дрон был полностью настроен, то вот такая последовательность двух команд привела бы к старту двигателей и взлету дрона на высоту 20 м:

arm throttle
takeoff 20

Не откалиброванный автопилот не полетит, а покажет сообщения с причинами, почему он этого сделать не сможет.

А теперь – о самом полетном контроллере

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

Структурная схема управляющего ПО показана на рисунке

Как видно из рисунка, ПО состоит из двух частей:

Так как тратить много времени не хотелось, то драйвер для USB в STM32 был взят прямо нативный и при помощи Interop был слинкован с оберткой на Ada.

Плата оказалась оснащена минимальным количеством периферийных устройств: 

Полетный контроллер реализован по простой схеме и «крутит» 2 цикла:

1. внешний

2. внутренний

Внешний цикл это цикл опроса периферии (CMD task на рисунке) в ожидании команд с радиопередатчика. Если команды нет, он передает признаки «сохраняем высоту, держим горизонт». Если команда с пульта есть, передаем ее – целевой угол наклона, целевую мощность на пропеллеры. Частота внешнего цикла 20 Гц.

Внутренний цикл – цикл опроса гиро-акселерометра и распределения мощности на двигатели. Цикл оборудован 3 PID-регуляторами, и математикой Махони для расчета текущего положения по сигналам с гироскопов. В расчетах внутри используем кватернионы, для генерации управляющего сигнала – углы Эйлера. Частота размыкания внутреннего цикла – 200 Гц. Да, Ада без проблем успевает диспетчеризировать с такой скоростью.

Так как у нас нет ни баровысотомера, и другого способа измерить высоту, то для грубого удержания высоты используем интеграцию вертикальной скорости.

Внутренний цикл реализует опрос PID и стабилизацию аппарата:

Забавно, что большинство опен-сорсных реализаций Махони (для Arduino и не только) – на Cи и Wiring оказались содержащими разнообразные баги. Это мешало системе заработать. После того, как было выпито пол-ящика лимонада и съедена корзина круассанов, алгоритм воссоздали с нуля по описанию из [MHN], и система тут же заработала.

Данная схема, как и любая упрощенная модель, испытывает сложности при приближениях параметров к предельным. Здесь это 90 по крену и тангажу – при их превышении для безопасности реализовано отключение двигателей (disarm).

Кроме этого, управление по углу рыскания выполнено сквозным, а PID там используется только для контроля сильных отклонений между выданным и расчетным углами. Это связано с тем, что пилот по скорости, крену и тангажу ожидает реакцию коптера в виде наклона, пропорционального отклонению стиков, а по рысканию — в виде скорости вращения, пропорциональной отклонению.

Но для первого приближения вышло отлично, хотя и совсем не подходит для акробатического квадрокоптера.

Статья получилась неожиданно длинная, поэтому придется разбить ее на две части.  Мы в первой части постарались познакомить вас с инструментами предков, которые на удивление неплохи: по крайней мере, мы сделали выводы, что дальнейшие проекты для эмбеддед хочется делать на Ada, а не на Си.

Итог на текущий момент

Квадрокоптер с прошивкой на Ada/SPARK прошел тесты на подпружиненном стенде и полетах в закрытом помещении, собираются логи, схема стабилизации работает в соответствии с ожиданиями, но в рамках ограничений на углы маневров, как описано выше.

Мы в R&D продолжаем познавать верификацию ПО и с нетерпением ждем схода снега в Москве, чтобы в следующей статье поделиться результатами испытаний, подгонки и видео тестовых полётов ПО в уличных условиях, которое, кажется, статически проверено на то, что не содержит ошибок.

И конечно, на время испытательных полетов все runtime-проверки все равно останутся включены, хотя конечный итоговый результат – проверки не нужны, так как заведомо известно, что они выполняются.

Для себя мы сделали вывод, что для embedded будем стараться писать только на Ada.

Видеотрансляция

Проверим как работает видеотрансляция в сети WiFi. Такой командой можно запустить видео в TCP-порт на Raspberry с использованием родной утилиты raspivid для камеры Raspicam:

raspivid -t 0 -hf -fps 25 -w 640 -h 480 -o - | gst-launch-1.0 fdsrc ! h264parse ! rtph264pay config-interval=1 pt=96 ! gdppay ! tcpserversink host=0.0.0.0 port=5001

А вот такой командой делается тоже самое, только с использованием ранее скомпилированной обертки rpi-camsrc для gstreamer:

gst-launch-1.0 rpicamsrc sensor-mode=4 ! h264parse ! rtph264pay config-interval=1 pt=96 ! gdppay ! tcpserversink host=0.0.0.0 port=5001

В обоих случаях, трансляция в формате h264 доступна по IP-адресу Raspberry на порту 5001.

Посмотреть ее можно запустив на своем ПК такую команду (должен быть установлен gstreamer), вместо RPI_ADDRESS указываем адрес Raspberry в сети:

gst-launch-1.0 -v tcpclientsrc host=RPI_ADDRESS port=5001  ! gdpdepay !  rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false

В результате должно открыться окошко с видео.

Практически в любую GCS встроен видеоплеер, который может показывать RTSP-видеопоток. Чтобы сделать из Raspberry RTSP-сервер можно использовать консольный плеер VLC. Установка:

sudo apt-get install vlc

Видеотрансляция запускается так:

raspivid -o - -t 0 -n -w 320 -h 240 -fps 25 | cvlc -vvv stream:///dev/stdin --sout '#rtp{sdp=rtsp://:8554/live}' :demux=h264

Видео доступно по адресу (вместо

RPI_ADDRESS

, адрес Raspberry):

rtsp://RPI_ADDRESS:8554/live

Настройка GCS:

Адрес потока можно использовать для подключения нескольких плееров на разных устройствах, но, так как видеозахват и трансляция для Raspberry весьма трудоемкий процесс, то для нескольких потребителей видео лучше использовать внешний сервер (описание ниже).

Гоночные полетные контроллеры

Обычно имеют минимум расширенных функций, так как всякие компасы и барометры просто не используются при гонках.

Naze32, также на базе этого контроллера есть SP Racing F3:

На нем присутствуют все стандартные датчики – гироскоп и акселерометр, а в расширенной версии DELUXE также есть барометр и компас.

Гироскоп и акселерометр определяют текущее расположение дрона в пространстве. Барометр определяет высоту по давлению (чтобы удерживать высоту, например), компас для удержания направления полета.

На сегодня, полетные контроллеры серии F4 являются самыми популярными полетными контроллерами для мини и гоночных квадрокоптеров, так как прекрасно работают с такими программами, как CleanFlight, Betaflight и Raceflight. На их смену уже выходит серия F7, становясь все более популярной.

Разработка прошивок для полетного контроллера F3 уже прекратилась из-за ограничения ресурсов, поэтому выбирайте для покупки F4 или F7:

Betaflight прекращает разработку ПО для полетных контроллеров F3 c STM32F3

Также еще два популярных контроллера:

KISS – прошивать своей прошивкой нельзя. Имеет графический интерфейс с минимумом настроек.

LUX – такой же гибкий, как Naze32, но все же уступает ему. Прошивать можно.

Дополнительные соображения

Функциональность: Производители полётных контроллеров, обычно, стараются предоставить как можно больше функций — либо включены по умолчанию, либо приобретаются отдельно в качестве опций/дополнений. Ниже приведены лишь некоторые из множества дополнительных функций, на которые вы, возможно, захотите взглянуть при сравнении контроллеров полёта.

Демпфирование: даже небольшие вибрации в раме, обычно вызываемые несбалансированными несущими винтами и/или моторами, могут быть выявлены встроенным акселерометром, который, в свою очередь, отправит соответствующие сигналы на главный процессор, который предпримет корректирующие действия. Эти незначительные исправления не нужны или не желательны для стабильного полёта, и лучше всего, чтобы контроллер полёта вибрировал как можно меньше. По этой причине между контроллером полёта и рамой часто используются виброгасители/демпферы.

Корпус: защитный корпус вокруг контроллера полёта может помочь в различных ситуациях. Помимо того, что корпус выглядит более эстетично, чем голая печатная плата, корпус часто обеспечивает некоторый уровень защиты элект. элементов, а также дополнительную защиту в случае краша.

Монтаж: Существуют различные способы установки контроллера полёта на раму, и не все контроллеры полёта имеют одинаковые варианты монтажа:

1. Четыре отверстия на расстоянии 30.5мм или 45мм друг от друга в квадрате.
2. Плоская нижняя часть для использования с наклейкой.
3. Четыре отверстия в прямоугольнике (стандарт не установлен).

Сообщество: поскольку вы создаете кастомный дрон, участие в онлайн-сообществе может значительно помочь, особенно, если вы столкнулись с проблемами или хотите получить совет. Получение рекомендаций от сообщества или просмотр отзывов пользователей, касательно качества и простоты использования различных контроллеров полёта, может также быть полезным.

Аксессуары: Для полноценного использования продукта, помимо самого контроллера полёта, могут потребоваться сопутствующие элементы (аксессуары или опции). Такие аксессуары могут включать, но не ограничиваются ими: модуль GPS и/или GPS антенна; кабели; монтажные принадлежности; экран (LCD/OLED);

Инвертирование сигнала последовательного порта

Процессоры F3 и F7 могут инвертировать сигнал встроенным инвертором, а F1 и F4 — нет.

Сигналы Frsky SBUS и SmartPort являются инвертированными, поэтому владельцам ПК на F3 и F7 повезло, такие данные понимаются без проблем (F3 и F7 — более новые серии процессоров, подробнее тут).

Однако, более старые процессоры, типа F1 и F4 требуют наличия внешнего инвертора сигнала, который и подключается к соответствующему последовательному порту. Для удобства пользователей некоторые ПК на F4 уже имеют схемы для инверсии сигналов SBUS и SmartPort, так что приемник подключается напрямую к ПК.

Если портов не хватает, можно использовать программную эмуляцию (soft serial) чтобы «создать» ещё больше портов. К сожалению, эмулируемые порты работают медленнее аппаратных (нельзя выставить большую скорость) и не подходят для важных задач, где требуется быстрая реакция, например не подойдут для работы с приемниками. Ну и, конечно, программная эмуляция требует довольно много ресурсов процессора.

Иной подход

Иной подход это верификация ПО. Обычно при этих словах люди начинают думать об абстрактных монадах и академических манускриптах, сложно читаемых и плохо применимых на практике. Но оказалось, что не все так плохо.

Прежде всего, верификация НЕ является гарантией того, что программа не содержит ошибок, а является только проверкой, что программа гарантирует некоторые свойства. А уже дело программиста таким образом обеспечить контроль свойств, чтобы получить нужные результаты.

В случае с SPARK, верификация базово предоставляет нам гарантии:

Для описания инвариантов в языке SPARK предусмотрены специальные расширяющие конструкции языка, описывающие контракты процедур, структур данных, и даже циклов. Контрактом можно указать, например, что данная функция не может обращаться к глобальным переменным, или модифицировать глобальное состояние.

SPARK также учитывает ограничения на типы, которые описаны в Ada. В случае обычного исполнения ошибка несоответствия типов упадет в Runtime; SPARK же позволяет статически доказать, что ограничения на типы не могут быть нарушены никаким потоком исполнения.

Например:

Или другой пример:

Компилятор и верификатор не дадут создать такой код, который приведет к присвоению значений, нарушающих ограничения и предикаты.

Отдельный плюс SPARK в том, что система позволяет “натягивать” гарантии на программу поэтапно, то есть программа может быть частично верифицированной. То есть часть модулей можно объявить верифицируемыми, а часть – (пока) нет.

Сам SPARK делит верификацию на уровни: от “каменного” (Stone level) через “Бронзовый” и “Серебряный” уровни до “Золотого” (Gold) и “Платинового”. Каждый из уровней усиливает гарантии:

Мы остановились на уровне Gold, потому что наш квадрокоптер все-таки не Boeing 777 MAX. 

Как работает верификация в SPARK: прувер собирает описание контрактов и типов, на их основе генерирует правила и ограничения, и далее передает их в солвер (SMT – Z3), который проверяет выполнимость ограничений. Результат решения прувер привязывает к конкретным строкам, в которых возникает невыполнимость.

Более подробно можно почитать в [SUG]

Иной язык

Несмотря на то, что сейчас “рулят” си-подобные ECMA-языки, мы нормально отнеслись к тому, что от этого придется отказаться. Более того, кажется, что чем больше программа, тем больше вредит укорочение ключевых слов и конструкций. Что касается Rust, то он – субъективно – в отношении минимализма издалека сильно напоминает Perl, к сожалению.

И наоборот, по ощущениям, пространные, многобуквенные конструкции раздражают, когда разум летит вперед, но не мешают, когда во главу угла ставится надежность, понятность, и легкость чтения. В этом смысле Ada (а SPARK это подмножество Ada) вполне хорош. Теперь давайте посмотрим, что же язык Ada может дать embedded-разработчику.

 Профили

Сам язык и стандартная библиотека позволяет определять и использовать так называемые “профили”. Профиль это набор ограничений, выполнение которых контролирует компилятор. Например, можно определить ограничение “нельзя использовать динамическую память”.

Мы используем профиль Ravenscar, специально для embedded-разработки. Он включает пару дюжин ограничений, которые делают вашу разработку для микроконтроллеров более удобной и верифицируемой: нельзя на ходу переназначать приоритеты задач-потоков, переключать обработчики прерываний, сложные объекты из stdlib-ы и такое.

Вот ограничения профиля Ravescar, для примера

 Runtime

Когда в embedded необходимо создать более-менее сложное приложение, там всегда заводится RTOS, и ее выбор и интеграция это отдельная песня. В случае с Ada с этим больше нет сложностей – сама Ada включает в себя минимальную исполняемую среду с вытесняющим планировщиком потоков (в Ada это tasks, задачи), с интегрированными в сам язык средствами синхронизации (семафоры, рандеву, называются entries) и даже средствами избегания дедлоков и инверсии приоритетов. Это оказалось очень удобно для квадрокоптера, как станет понятно ниже.

Для embedded-разработчика доступны на выбор также разные рантаймы:

Вот пример описания пустой задачи

Хочется обратить внимание, что эти задачи – это по сути легковесные green threads, так как под ними нет никаких настоящих потоков не существует. Поэтому мы не страдаем от отсутствия корутин, ведь задачи не тяжелее них, зато встроены в язык.

Кроме этого, в Ada есть достаточно мощная stdlib для ядра STM32, включая полную реализацию рантайма. Возможно, и для вашей архитектуры она тоже есть.

Почему не “rustRustRUST”!

Когда мы говорим, про гарантии в языках программирования, сразу вспоминается Rust и его гарантии относительно указателей. Почему тут не он? Нам кажется, что Spark мощнее.

Ada не очень любит указатели – там они называются access types, и в большинстве случаев там они не нужны, но если нужны, то – в Spark также есть проверки владения, как в Rust. Если вы аллоцировали объект по указателю, то простое копирование указателя означает передачу владения (которую проконтролирует компилятор/верификатор), а передачу во временное владение (или доступ на чтение) верификатор также понимает и контролирует.

В общем, концепция владения объектом по указателю, и уровень доступа через этот указатель – есть не только в Rust, и его преимуществами можно пользоваться и в других инструментах, в частности, в Ada/SPARK. Подробно можно почитать в [UPS]

Вот пример кода с владением

Почему мы пишем, что в Ada/SPARK не нужны указатели? В отличие от Си, где базовым инструментом является указатель (хочешь ссылку – вот указатель, хочешь адрес в памяти – вот указатель, хочешь массив – вот указатель – ну вы поняли?), в Ada для всего этого есть строгий тип.

Если уже и этого мало, есть интероп с СИ – то есть код можно компилировать совместно, и слинковать на этапе сборки. Но в этом случае гарантии поведения модуля как черного ящика остаются на разработчике. Для интеропа используются атрибуты – вот так, например, мы оборачиваем функцию на Си в доступ для Ada.

Для соблюдения нужного layout или битности в коде также не нужны указатели: Ада при необходимости позволяет детально описать, как именно структура будет располагаться в памяти. Минус ошибки на конвертации из логического в физическое представления и обратно – прощайте битовые сдвиги, сложения на кольце, арифметика указателей.

IDE

Для работы доступна вполне приятная и удобная IDE, но всегда можно использовать и VSCode с плагинами, и другие текстовые редакторы.

О производительности и надежности

Вполне валидным аргументом может быть вопрос с эффективностью ПО. Что касается эффективности, то в интернете доступно свежее исследование [EFF], из которого хочется привести табличку, показывающую, что “старичок» Ada еще огого:

Если говорить о надежности, то SPARK/Ada известен как один из языков с наименьшим количеством ошибок. В планируемом на 21 запуске кубсатов [LIC] полетное ПО планируется реализовывать на Ada, предыдущий спутник BasiLEO тоже на Ada был единственным среди 12, кому удалось приступить к планируемой миссии.

Калибровка датчиков и настройка параметров автопилота

Калибровку автопилота можно сделать почти в любой GCS. В документации Ardupilot она

во всех подробностях. Прежде всего устанавливаем тип рамы. У меня стандартная 4-х моторная компоновка, поэтому это

Quad X

Первый полет лучше все же сделать в ручном режиме. Подключаем и калибруем радиоуправление (приемник и передатчик).

Осталось откалибровать акселерометр и компас.

Для того, чтобы Ardupilot видел и учитывал данные с внешних датчиков, установим необходимые параметры:

Для PX4Flow (калибровка самого датчика и обновление прошивки)

FLOW_ENABLE = 1 (Enabled)FLOW_ADDR = 0 (0 = вариант для стандартного адреса 0х42)

Для лазерного высотомера VL53L0X (инструкция)

RNGFND_TYPE = 16 (VL53L0X)RNGFND_ORIENT = 25 (ориентация дальномера вниз)RNGFND_ADDR = 41 (I2C-адрес в десятичном виде). Адрес датчика по-умолчанию 0x29, что в десятичном виде = 41.RNGFND_SCALING = 1RNGFND_MIN_CM = 5RNGFND_MAX_CM = 120RNGFND_GNDCLEAR = 15 (расстояние от датчика до поверхности, когда дрон стоит на земле)

Для IRLock (подробная инструкция, wiki IR-Lock)

PLND_ENABLED = 1PLND_TYPE = 2PLND_BUS = 1

Для сонара переднего обзора (инструкция)

Настройка и запуск ardupilot

Релизы новых версий Ardupilot немного запаздывают в сборке от Emlid. Если необходимый функционал доступен в самой последней версии, то установить ее из исходников можно

Разработчики Navio добавили в свою сборку простую и удобную утилиту Emlid tool для проверки датчиков и настройки Ardupilot. Сначала проверим, видит ли Raspberry контроллер Navio:

emlidtool info

Если в ответ на эту команду выдает что-то вроде:

Vendor: Emlid Limited
Product: Navio 2
Issue: Emlid 2023-06-05 831f3b08594f2da17dccae980a2e3659115ef71f
Kernel: 4.14.34-emlid-v7 
RCIO firmware: 0xcaec2284

значит видит. Проверим состояние датчиков (покажет список и состояние):

emlidtool test

и драйвера ШИМ-контроллера в ядре Linux:

cat /sys/kernel/rcio/status/alive

0 = не работает, 1 = работает.

Прошивка ШИМ-контроллера обновляется так:

sudo emlidtool rcio update

Теперь настроим Ardupilot:

sudo emlidtool ardupilot

В терминале откроется текстовый GUI с пошаговыми менюшками. Выбираем copter последней версии, тип

arducopter

, автозапуск при включении (

On boot: enable

), старт после настройки (

Ardupilot: start

Выходим через пункт меню Quit.

Проверим запустился ли Ardupilot:

sudo systemctl status arducopter

Обратите внимание, файл запуска в systemd называется

arducopter

, так как настроен был вариант

copter

Теперь нужно настроить Ardupilot так, чтобы он отправлял нам телеметрию. Для этого отредактируем файл конфигурации:

sudo nano /etc/default/arducopter 

В нем должны быть такие строки:

TELEM1="-A udp:127.0.0.1:14550"
ARDUPILOT_OPTS="$TELEM1"

Сохраняем файл (

Ctrl X

, затем

Y

) и перезапускаем Ardupilot:

sudo systemctl daemon-reload
sudo systemctl restart arducopter

Проверить состояние процесса Ardupilot можно такой командой:

sudo systemctl status arducopter

С такими настройками Ardupilot будет транслировать телеметрию (пакеты

) в локальный UDP-порт 14550. Далее, скрипт

(описание ниже) будет забирать оттуда телеметрию и передавать в GCS или скрипт, а также отправлять в обратном направлении пакеты с командами.

Вместо локального адреса и порта можно записать IP-адрес ПК или планшета в локальной сети и пакеты будут транслироваться сразу туда.

Однако, такой подход оправдан, если данные телеметрии больше нигде не используются и у устройства с GCS статический IP адрес. Иначе каждый раз в настройках Ardupilot придется прописывать новый. Чтобы общаться с автопилотом по TCP могли одновременно несколько GCS с динамическими адресами и еще какие-нибудь скрипты на самом бортовом компьютере, удобнее использовать MAVProxy.

Этот скрипт (написан на Python) может получать пакеты MAVLink на локальный UDP-адрес и ретранслировать их на несколько локальных или удаленных IP-адресов как по UDP, так и по TCP. Пакеты передаются в обоих направлениях Ardupilot ⇔ GCS. Кроме того, MAVProxy представляет из себя полноценную GCS, но с текстовым интерфейсом.

Пид-регуляторы

При работе с мультикоптерами, вам придется столкнуться с настройкой ПИД-регулятора, этот математический аппарат применяется почти во всех задачах стабилизации: стабилизация углов квадрокоптера в воздухе, полет и удержание позиции по GPS, удержание высоты по альтиметру.

Вы собираете мультикоптер, калибруете датчики, регуляторы, радио, все проверяете, пытаетесь взлететь, а его переворачивает даже легким ветерком. Или наоборот: он такой резкий, что внезапно срывается с места и крутит тройное сальто без разрешения. Причина все та же: параметры ПИД-регуляторов.

Для многих устройств, использующих ПИД-регуляторы, существуют инструкции по настройке. Но чтобы легче ориентироваться в этом многообразии полезно понимать, как же внутри устроены эти регуляторы. Предлагаю вместе со мной самим заново «изобрести» и «на пальцах» понять формулу ПИД-регулятора.

В полетный контроллер непрерывно поступают команды с земли: «крен 30 градусов», «крен -10 градусов», «крен 0 градусов (держать горизонт)»; его задача — как можно быстрее и точнее их выполнять с помощью моторов с учетом: ветра, неравномерного распределения веса квадрокоптера, неравномерного износа моторов, инерции квадрокоптера и т.п.

Уровень газа поступает из приемника в контроллер. Обозначим его throttle. Если left и right — скорости вращения левого и правого моторов, то:

left = throttle force,
right = throttle – force,

где force — реакция квадрокоптера (усилие), которое создает момент вращения за счет того, что левый мотор вращается на force быстрее, чем газ, а правый — на столько же медленнее. force может принимать и отрицательные значения, тогда правый мотор закрутится быстрее.

Если мы научимся вычислять эту величину на каждой итерации цикла обработки, значит мы сможем управлять квадрокоптером. Понятно, что force как минимум должно зависеть от текущего угла крена (roll) и желаемого угла крена (tar get_roll), который поступает с пульта управления.

Представим ситуацию: поступает команда «держать горизонт» (tar get_roll = 0), а квадрокоптер имеет крен влево:

Рис. Двухмерный квадрокоптер с креном влево.
error — разность (ошибка) между tar get_roll и roll, которую контроллер стремится минимизировать.

Чем больше разность между желаемым углом крена и текущим, тем сильнее должна быть реакция, тем быстрее левый мотор должен закрутиться относительно правого. Если это записать с использованием наших обозначений:

force = P * error

P — коэффициент пропорциональности. Чем он больше, тем сильнее будет реакция, тем резче квадрокоптер будет реагировать на отклонение от требуемого угла крена. Эта интуитивно понятная и простая формула описывает работу пропорционального регулятора. Чем сильнее квадрокоптер отклонился от требуемого положения, тем сильнее надо пытаться его вернуть. К сожалению, эту формулу придется усложнить. Главная причина — перерегулирование.

За несколько десятков миллисекунд (несколько итераций цикла обработки) под воздействием пропорционального регулятора квадрокоптер вернется в требуемое (в данном случае горизонтальное) положение. Все это время ошибка error и усилие force будут иметь один и тот же знак, хоть и становиться все меньше по модулю.

Набрав какую-то скорость поворота (угловую скорость) квадрокоптер просто перевалится на другой бок, ведь никто его не остановит в требуемом положении. Все равно что пружина, которая всегда стремится вернуться в начальное положение, но если ее оттянуть и отпустить — будет колебаться, пока трение не возьмет верх.

По этой причине в пропорциональный регулятор нужно добавить еще одно слагаемое, которое будет тормозить вращение квадрокоптера и препятствовать перерегулированию (переваливанию в противоположную сторону) —имитация трения в вязкой среде: чем быстрее поворачивается квадрокоптер, тем сильнее надо пытаться его остановить, конечно, в разумных пределах. Скорость вращения (скорость изменения ошибки ) обозначим как spin, тогда:

D — настраиваемый коэффициент: чем он больше, тем сильнее останавливающее усилие.

Скорость изменения любой величины — производная этой величины по времени:

И вот пропорциональный регулятор превращается в пропорционально-дифференциальный (пропорциональное слагаемое и дифференциальное):

Ошибку error вычислить легко, ведь на каждой итерации мы знаем roll и tar get_roll; P и D — настраиваемые перед запуском параметры. Для вычисления производной (скорости изменения error) необходимо хранить предыдущее значение error, знать текущее значение error и знать время, которое прошло между измерениями (период регулирования). И вот она — физика шестого класса школы (скорость = расстояние / время):

dt — период регулирования; error previous — значение ошибки с предыдущей итерации цикла регуляции. Кстати, эта формула — простейший способ численного дифференцирования, и он нам здесь вполне подойдет.

Теперь у нас есть пропорционально-дифференциальный регулятор в плоском «бикоптере», но осталась еще одна проблема. Пусть левый край будет весить чуть больше правого, или, что то же самое, левый мотор работает чуть хуже правого. Квадрокоптер чуть наклонен влево и не поворачивается обратно: дифференциальное слагаемое равно нулю, а пропорциональное слагаемое хоть и принимает положительное значение, но его не хватает, чтобы вернуть квадрокоптер в горизонтальное положение, ведь левый край весит чуть больше правого. Как следствие — квадрокоптер будет все время тянуть влево.

Необходим механизм, который бы отслеживал такие отклонения и исправлял их. Характерной особенностью таких ошибок является то, что они прявляют себя со временем. На помощь приходит интегральное слагаемое. Оно хранит сумму всех ошибкок error по всем итерациям цикла обработки.

Как же это поможет? Если пропорционального слагаемого не достаточно, чтобы исправить маленькую ошибку, но она все равно есть — постепенно, со временем, набирает силы интегральное слагаемое, увеличивая реакцию force и квадрокоптер принимает требуемый угол крена.

Тут есть нюанс. Предположим error равна 1 градусу, цикл регулирования — 0.1с. Тогда за одну секунду сумма ошибок примет значение 10 градусов. А если цикл обработки — 0.01с, то сумма наберет аж 100 градусов. Чтобы за одно и тоже время интегральное слагаемое набирало одно и тоже значение при разных периодах регулирования, полученную сумму будем умножать на сам период регулирования.

Эта формула — не что иное, как численный интеграл по времени функции error в интервале от нуля до текущего момента. Именно поэтому слагаемое называется интегральным:

где T — текущий момент времени.

Пришло время записать окончательную формулу пропорционально-интергрально-дифференциального регулятора:

где I — один из настраиваемых параметров, которых теперь трое: P,I,D.
ПИД регуляторы – важная часть полётного контроллера, без их использования квадрокоптер летал бы непредсказуемо. Они настраиваются индивидуально для каждого квадрокоптера.

Пример

Итак, учитывая все эти различные сравнительные характеристики, какую информацию вы можете получить о контроллере полёта и что может включать контроллер полета? В качестве примера мы выбрали Quadrino Nano Flight Controller.

Главный процессор

Используемый на борту ATMel ATMega2560 является одним из наиболее мощных Arduino-совместимых чипов ATMel. Хотя он имеет в общей сложности 100 выводов, включая 16 аналогово-цифровых каналов и пять портов SPI, из-за его небольшого размера и предполагаемого использования в качестве контроллера полёта, на плате присутствуют только некоторые из них.

• AVR vs PIC: AVR
• Процессор: 8-бит
• Рабочая частота: 16МГц
• Программная память/Flash: 256Кбайт
• SRAM: 8Кбайт
• EEPROM: 4Кбайт
• Дополнительные контакты ввода/вывода: 3 × I2C; 1 × UART; 2 × 10-контактных GPIO; Серво с 5 × выходами; OLED порт
• Аналого-цифровой преобразователь: 10-бит

Сенсоры

Quadrino Nano включает микросхему MPU9150 IMU, которая включает в себя 3-осевой гироскоп, 3-осевой акселерометр и 3-осевой магнитометр. Это помогает сделать плату достаточно маленькой, не жертвуя качеством датчика. Барометр MS5611 предоставляет данные о давлении и покрыт кусочком пены. Интегрированный Venus 838FLPx GPS с внешней GPS антенной (в комплекте).

Программное обеспечение

Quadrino Nano был создан специально для использования новейшего программного обеспечения MultiWii (на базе Arduino). Вместо того, чтобы изменять код Arduino напрямую, было создано отдельное, более графическое программное обеспечение.

Связь

• Прямой ввод от стандартного RC приёмника.
• Порт выделенного спутникового ресивера Spektrum
• Последовательный (SBus и/или Bluetooth или 3DR радиосвязи)

Дополнительные факторы

1. Корпус: защитный полупрозрачный корпус входит в стандартную комплектацию
2. Монтаж: Есть два основных способа крепления Quadrino Nano к дрону: винты и гайки или наклейка из вспененной резины.
3. Компактная конструкция: сам контроллер (без учёта GPS антенны) имеет размеры 53 × 53мм.

Принцип функционирования полётного контроллера

Полётный контроллер – устройство, обеспечивающее полёт квадрокоптера, за счет управления газом, углами крена, тангажа и рысканья (throttle, pitch, roll, yaw). Это своеобразные “мозги” мультикоптера. Обычно он содержит несколько датчиков (гироскопы, акселерометр, магнитометр, GPS датчик) и микроконтроллер, который производит расчеты.

Полётный контроллер несколько десятков раз в секунду выполняет цикл управления в который входит: считывание показаний датчиков, считывание каналов управления, обработка информации и выдача управляющих сигналов моторам, чтобы выполнять команды пилота.

Полётный контроллер выполняет следующие задачи:

• Собирает информацию с датчиков (встроенные, либо внешние: гироскопы, акселерометры, GPS, магнитометр);
• Рассчитывает свое положение в пространстве, по показаниям датчиков;
• Собирает информацию о внешних воздействиях, таких как отклонения стиков пилотом, алгоритм программы;
• Вносит корректировку с помощью коэффициентов ПИД (Пропорционально-Интегрально-Дифференциальные);
• Отправляет управляющие сигналы на регуляторы оборотов (ESC).

Полётный контроллер выдает ШИМ-импульсы (PWM) на регуляторы оборотов (ESC), в зависимости от команды стика джойстика, либо программы. Например, чтобы дать команду мотору вращаться с максимальной скоростью контроллер должен отправлять импульсы длительностью 2 миллисекунды, перемежающиеся логическим нулем длительностью 10 — 20 миллисекунд.

Но все не так просто, полетные контроллеры бывают разные с разными настройками, регуляторы бывают разные, минимум (1 мс) и максимум (2 мс) — не универсальны. В зависимости от множества факторов диапазон 1-2 мс может на деле оказаться 1.1 — 1.9 мс, либо другим. Чтобы регулятор и контроллер говорили абсолютно на одном языке существует процедура калибровки регуляторов.

Режимы полёта

Ниже приведён список самых популярных режимов полёта, тем не менее не все из них могут быть доступны в полётных контроллерах. «Режим полёта» — это способ, посредством которого полётный контроллер использует сенсоры и входящие радиокоманды для обеспечения стабилизации и полёта БПЛА.

• ACRO — обычно режим по умолчанию, из всех имеющихся сенсоров, контроллером полёта задействуется только гироскоп (беспилотник не может автоматически выравниваться). Актуален для спортивного (акробатического) полёта.

• ANGLE — стабильный режим; из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп и акселерометр. Углы ограничены. Будет удерживать беспилотник в горизонтальном положении (но без удержания позиции).

• HORIZON — сочетает в себе стабильность режима «ANGLE», когда стики находятся вблизи центра и перемещаются медленно, и акробатику режима «ACRO», когда стики находятся в крайних положениях и перемещаются быстро. Контроллером полёта задействуется только гироскоп.

• BARO (Altitude Hold) — стабильный режим; из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр и барометр. Углы ограничены. Барометр используется для удержания определенной (фиксированной) высоты, когда с аппаратуры управления не подаются никакие команды.

• MAG (Heading Hold) — режим блокировки курса (направления компаса), беспилотник будет сохранять Yaw ориентацию. Из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр и компас.

• HEADFREE (CareFree, Headless, Безголовый) — исключает отслеживание ориентации (Yaw) дрона и тем самым позволяет перемещаться в 2D направлении согласно перемещению стика управления ROLL/PITCH. Из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр и компас.

• GPS/Return to Home — автоматически использует компас и GPS, чтобы вернуться к месту взлёта. Из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр, компас, и модуль GPS.

• GPS/Waypoint — позволяет беспилотнику автономно следовать по предварительно установленным GPS точкам. Из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр, компас, и модуль GPS.

• GPS/Position Hold — удерживает текущую позицию с помощью GPS и барометра (если доступен). Из всех имеющихся сенсоров, контроллером полёта задействуются гироскоп, акселерометр, компас, и модуль GPS.

• Failsafe (аварийный/отказоустойчивый режим) — если другие режимы полёта заданы не были, беспилотник переходит в режим Acro. Из всех имеющихся сенсоров, контроллером полёта задействуется только гироскоп. Актуален при сбоях в программном обеспечении беспилотника, позволяет восстановить контроль над БЛА посредством ранее предустановленных команд.

Связь

Радиоуправление (RC)

Управление посредством радиосвязи обычно включает в себя RC передатчик/RC transmitter (в беспилотном хобби — радиоаппаратура управления/пульт) и RC приёмник (RC receiver). Для взаимодействия с БПЛА пользователю потребуется как минимум четырёх (и более) канальный RC передатчик. По умолчанию первые четыре канала связаны с:

Все остальные имеющиеся каналы могут быть задействованы для таких действий как:

• Арминг (Arming или Arm)/Дизарминг (Disarming или Disarm) — постановка/снятие с охраны моторов.
• Управление подвесом (панорамирование вверх/вниз, вращение по часовой стрелке/против часовой стрелки, зуммирование)
• Смена режимов полёта (ACRO/ANGLE и т.д.)
• Активировать/Задействовать полезную нагрузку (парашют, зуммер или другое устройство)
• Любое другое применение

Большинство пользователей (пилотов БПЛА) предпочитают именно ручное управление, это ещё раз доказывает, что пилотирование при помощи аппаратуры управления по прежнему является выбором номер один. Сам по себе RC приёмник просто передаёт поступающие от RC передатчика значения, а значит не может управлять беспилотником. RC приёмник должен быть подключен к контроллеру полёта, который в свою очередь должен быть запрограммирован для приёма RC сигналов. На рынке очень мало полётных контроллеров, которые принимают входящие радиокоманды от приёмника на прямую, а большинство ПК даже обеспечивают питание приёмника от одного из контактных выводов. Дополнительные соображения при выборе пульта дистанционного управления включают в себя:

• Не все RC передатчики могут обеспечить полный диапазон RC сигналов от 500мс до 2500мс; некоторые искусственно ограничивают этот диапазон, так как большинство используемых RC предназначены для радиоуправляемых автомобилей, самолётов и вертолётов.
• Дальность/Макс. воздушный радиус действия (измеряется в футах или метрах) RC-системы — практически никогда не предоставляются производителями, поскольку на этот параметр влияют множество факторов, таких как помехи, температура, влажность, заряд батареи и другие.
• Некоторые RC-системы имеют приёмник, который также имеет встроенный передатчик для передачи данных от датчика (например, GPS-координат), которые в последствии будут отображаться на ЖК-дисплее RC передатчика.

Bluetooth

Bluetooth и более поздние продукты BLE (Bluetooth Low Energy) изначально предназначались для передачи данных между устройствами без заморочек сопряжения или согласования частот. Некоторые имеющиеся на рынке контроллеры полёта могут отправлять и получать данные по беспроводной связи через соединение Bluetooth, что упрощает поиск неисправностей в полевых условиях.

Wi-Fi

Управление по Wi-Fi обычно достигается посредством Wi-Fi роутера, компьютера (в том числе ноутбук, десктоп, планшет) или смартфон. Wi-Fi в состоянии справится как с передачей данных, так и с передачей видеопотока, но одновременно с этим эту технологию сложнее настроить/реализовать. Как и для всех Wi-Fi устройств, расстояние удаления ограничено Wi-Fi передатчиком.

Радиочастота (RF или РЧ)

Радиочастотное (РЧ) управление в этом контексте относится к беспроводной передаче данных с компьютера или микроконтроллера на летательный аппарат с использованием РЧ передатчика/Приёмника (или двухполосного приёмопередатчика). Использование обычного радиочастотного блока, подключенного к компьютеру, позволяет осуществлять двухполосную связь на большие расстояния с высокой «плотностью» данных (обычно в последовательном формате).

Смартфон

Хоть это и не тип связи, самого вопроса, как управлять дроном используя смартфон, достаточно, чтобы уделить ему отдельный раздел. Современные смартфоны это по сути мощные компьютеры, которые по случайному совпадению могут также совершать телефонные звонки. Почти все смартфоны имеют встроенный модуль Bluetooth, а также модуль WiFi, каждый из которых используется для управления дроном и/или получения данных и/или видео.

Инфракрасное излучение (Infrared (IR))

Инфракрасная связь (то что можно найти в каждом телевизионном пульте дистанционного управления) редко используется для управления дронами, так как даже в обычных комнатах (не говоря уже об открытом пространстве) присутствует так много инфракрасных помех, что они не очень надёжны. Несмотря на то, что технологию можно использовать для управления БПЛА, не может быть предложена как основной вариант.

Сенсоры

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

• Акселерометр: Как следует из названия, акселерометры измеряют линейное ускорение по трем осям (назовём их: X, Y и Z). Обычно измеряется в «G (на рус. Же)». Стандартное (нормальное) значение, составляет g = 9.80665 м/с². Для определения положения, выход акселерометра может быть интегрирован дважды, правда из-за потерь на выходе объект может быть подвержен дрейфу. Самой значимой характеристикой трёхосевых акселерометров является то, что они регистрируют гравитацию, и как таковые, могут знать, в каком направлении «спуск». Это играет главную роль в обеспечении стабильности многороторного БЛА. Акселерометр должен быть установлен на контроллере полёта так, чтобы линейные оси совпадали с основными осями беспилотника.

• Гироскоп: Гироскоп измеряет скорость изменения углов по трём угловым осям (назовём их: альфа, бета и гамма). Обычно измеряется в градусах в секунду. Обратите внимание, что гироскоп не измеряет абсолютные углы напрямую, но вы можете выполнить итерацию, чтобы получить угол, который, как и у акселерометра, способствует дрейфу. Выход реального гироскопа имеет тенденцию быть аналоговым или I2C, но в большинстве случаев вам не нужно беспокоиться об этом, так как все поступающие данные обрабатываются кодом контроллера полёта. Гироскоп должен быть установлен так, чтобы его оси вращения совпадали с осями БПЛА.

• Инерционный измерительный блок (IMU): IMU — по сути, это небольшая плата, которая содержит как акселерометр, так и гироскоп (обычно многоосевые). Большинство из них включают трёхосевой акселерометр и трёхосевой гироскоп, другие могут включать дополнительные сенсоры, например трёхосевой магнитометр, обеспечивающий в общей сложности 9 осей измерения.

• Компас/Магнитометр: Электронный магнитный компас способный определять магнитное поле Земли и использовать эти данные для определения направления компаса беспилотника (относительно северного магнитного полюса). Этот сенсор почти всегда присутствует, если система имеет GPS вход и доступно от одной до трех осей.

• Давление/Барометр: Так как атмосферное давление изменяется по мере удаления от уровня моря, можно использовать сенсор давления, чтобы получить довольно точные показания высоты БПЛА. Для расчёта максимально точной высоты, большинство контроллеров полёта получают данные одновременно от сенсора давления и спутниковой системы навигации (GPS). При сборке обратите внимание, что предпочтительнее, чтобы отверстие в корпусе барометра было накрыто куском поролона, это уменьшить отрицательное влияние ветра на чип.

• Расстояние: Датчики расстояния все чаще используются на беспилотниках, поскольку GPS-координаты и датчики давления не могут рассказать вам, насколько далеко вы находитесь от земли (холма, горы или здания), либо столкнётесь ли вы с объектом или нет. Датчик расстояния, обращенный вниз, может быть основан на ультразвуковой, лазерной или лидарной технологии (ИК-сенсоры могут испытывать проблемы в работе при солнечном свете). Датчики расстояния редко входят в стандартный комплект полётного контроллера.