Px4 ahrs error

Статья в формате «Редакция выступила. Что сделано?». Год назад мы рассказывали, как и почему используем стандарт UAVCAN и единую шину данных для наших проектов с беспилотниками. Одной из фишек, изложенных на уровне концепции, была true HITL-симуляция. Что ж, пора от концепции перейти к представлению проекта. Мы разработали новый вариант HITL-симуляции, когда автопилот может даже не знать, работает ли он с реальными датчиками на шине или же в симулируемом окружении. Набор программных модулей, работающих в «боевом» режиме и в режиме симуляции (почти) идентичен, в отличие от альтернативного MAVLINK-HITL подхода.

Stand on the shoulders of giants

Этот проект — наш вклад в экосистему вокруг PX4 и UAVCAN, которая нам очень помогает в работе. Он опубликован на github и доступен всем желающим: https://github.com/InnopolisAero/innopolis_vtol_dynamics. Будем рады адоптерам и контрибьюторам.

Вокруг

Про вопросы надежности и ответственность при разработке систем управления для беспилотников, особенно летающих, сказано много, в том числе здесь и тут. Часть этих рассуждений (например, идея применения для разработки встраиваемого ПО подходов из области web-разработки) очень дискуссионная, если не сказать холиварная. Но то, что нужна единая шина данных и связанная с этим возможность создания распределенных систем управления, думаю, уже не должно вызывать сомнений.

Я всё же опишу некоторые моменты, которые хоть напрямую не относятся к теме заголовка, но составляют существенную доли мотивации этого проекта. Надежность по-прежнему является барьером для широкого коммерческого применения беспилотников за границами видимости оператора (BVLOS — Beyond Visual Line of Sight), для доставки последней мили, например. Посмотрим на лидеров:

Даже по внешнему виду аппаратов понятно, что разработчики думают о повышении надежности за счет избыточности (redundancy) органов управления. Но надежность на исполнительных механизмах не останавливается: в идеале нужно резервировать и исполнительные механизмы и все основные сенсоры и контроллеры.Такого в популярных opensource автопилотах нет и все разработчики или делают это долго и дорого сами или не делают. Что мешает сделать автопилот аэротакси на PX4? За такое предложение в некоторых местах могут и побить. Но, на мой взгляд, как раз таки развивая в PX4 тему с распределёнными бортовыми системами, можно этого добиться. И тогда (потенциально) можно сделать автопилот аэротакси по цене сравнимой с автопилотом дрона (или хотя бы того же порядка).

Хорошо, допустим, мы сделали распределенную систему с резервированием на UAVCAN шине. Как это все отлаживать? Для тестов нам нужно не меньше 50 дронов в качестве расходников. Еще одно преимущество архитектуры: чтобы уменьшить количество неудачных лётных тестов, мы можем сначала их (испытания) симулировать. Причем мы можем симулировать наши датчики и исполнительные механизмы на шине практически неотличимо от настоящих устройств, просто подключив симулятор к той же шине. Если бы мы то же самое хотели сделать для архитектуры на картинке слева, то нам пришлось бы эмулировать 3-5 различных интерфейсов и еще больше их портов и протоколов.

Проблема

Как делается HITL-симуляция в PX4 (и Ardupilot) сейчас? Несколько различных интерфейсов, портов и протоколов никто (ну или почти никто) не симулирует. А используют один интерфейс: UART и MAVLINK в нём. Интро по тому, как это сейчас делается в PX4, тут. Вот только программная конфигурация автопилота, работающего в режиме такой симуляции, существенно отличается от лётной конфигурации: работает разный набор драйверов и программных модулей. Такая симуляции неполная. К тому же, она не отвечает тенденциям перехода к единой бортовой шине данных.

Решение

В целом, из пространного введения уже понятно, что нужно делать. Оставалось, собственно, сделать. Симулятор состоит из следующих компонентов:

• матчасть, UavDynamics — симулятор динамики БПЛА (на примере Innopolis VTOL: динамика квадрокоптера и самолета с симуляцией аэродинамики);

• коммуникатор, Communicators — пакет обмена сообщениями с полетным стеком PX4 в режимах HITL через UAVCAN и SITL через MAVLink;

• красота, inno_sim_interface — мост для взаимодействия с трехмерным визуализатором InnoSimulator через ROS.

Имманентными компонентами симулятора обычно являются модули моделирования динамики (физики) и трёхмерного рендеринга. На рисунке выше они названы UavDynamics и Innopolis Simulator. Про нашу реализацию динамики я расскажу дополнительно, но в целом, это может быть и на базе Gazebo. Для реализации подключения к шине UAVCAN был разработан пакет Communicators. Он содержит как реализацию симуляции по MAVLINK для совместимости, так и UAVCAN-шлюз uavcan_communicator. Для работы с CAN-шиной с персонального компьютера (на котором запускается симуляция) нужно примерно такое устройство:

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

Подопытный

На примере всегда понятнее, поэтому вот он: симулятор был протестирован на Innopolis VTOL, который может быть классифицирован как quadplane (самолёто-квадрокоптер). Он оснащен автопилотом PX4 CUAV v5+, который осуществляет управление тремя управляющими поверхностями (элеронами с обеих сторон крыла, рулями высоты и направления) посредством сервоприводов (естественно, с UAVCAN), а также 4 коптерными BLDC-двигателями с помощью электронных регуляторов скорости (ESC) с интерфейсом UAVCAN и маршевым самолетным двигателем: BLDC с ESC через UAVCAN либо ДВС (управление заслонкой сервой). Он также содержит следующий набор датчиков на базе UAVCAN: датчик воздушной скорости, барометр, приёмник GPS и компас, лазерный высотомер, датчик уровня топлива, датчики системы электропитания, обратная связь от ESC и ДВС (обороты). В симуляторе мы стремились создать наиболее близкую виртуальную модель данного БПЛА как по составу так и по динамике. Также в качестве альтернативного примера поддерживается модель динамики коптера на базе FlightGoggles.

Динамика

Что касается динамики, то в общем случае для ROS есть неплохой инструмент — Gazebo. Это такой конструктор моделей, в нём в качестве базовых блоков есть и двигатели и твердое тело и аэродинамика. Но, если хочется большей кастомизации или лучшей графики, то он не годится.

К примеру, аэродинамика в Gazebo моделируется двумя линейными характеристиками. Мы в этом вопросе немного больше заморачивались для создания более физичной модели именно нашего аппарата (см. ниже) и нам нужны более сложные нелинейные зависимости. К тому же, у нас есть Innopolis Simulator с крутой графикой.

Так что от Gazebo мы отказались и сделали свою симуляцию динамики.

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

Динамика отвечает за изменение скоростей под воздействием сил и моментов. Это немного сложнее законов Ньютона и уравнения поступательного движения материальной точки из-за того, что учитывается вращение, но ассоциации годные.

Индивидуальность моделей разных аппаратов заключаются в параметрах (массо-инерционные характеристики) и силах, которые действуют на БПЛА:

• управляющие воздействия от органов управления;

• аэродинамика;

• сила тяжести и плавучесть (если хочется экзотики).

Аэродинамика

Аэродинамическая сила представляет собой сумму подъемной силы, силы сопротивления и силы бокового скольжения, которые могут быть рассчитаны с помощью следующих уравнений соответственно:

Аэродинамический момент делится на 2 составляющие: первая зависит от модуля воздушной скорости и угла атаки, вторая — от воздушной скорости и положения управляющих поверхностей:

Аэродинамические коэффициенты мы получали посредством CFD (computational fluid dynamics). Результатом CFD-исследования являются массивы данных, описывающие основные аэродинамические характеристики рассматриваемого самолета вертикального взлета и посадки с учетом его отклонения по углу атаки и скольжения и отклонения рулевых поверхностей в рассматриваемом диапазоне скоростей. Также исследовались градиенты распределения полей давления, скоростей и турбулентных потоков в окрестности аппарата.

Актуаторы

Тягу движителя можно моделировать как функцию, которая получает скорость двигателя и возвращает силу и момент на основе табличной зависимости (статические характеристик). Эта таблица может быть получена посредством CFD-анализа или экспериментально на примерно таком стенде:

Что касается динамики движителя, можем считать, что передаточная функция регулятора двигателя BLDC и ESC может быть описана как

Модели сенсоров

Для моделирования датчика воздушной скорости и барометра используется модель стандартной атмосферы ISA (InternationalStandard Atmosphere). Акселерометр и гироскоп моделируются в соответствии с выражениями:

UavDynamics (inno_vtol)

inno_vtol — это отдельный узел ROS, который реализует описанную выше математику. Он осуществляет связь через топики, и не имеет значения, какой тип связи со стеком полётного контроллера вы используете. uav_dynamics публикует и подписывается на следующие топики:

Как видите, у нас есть обратная связь от регуляторов подъемных двигателей и ДВС.

uavcan_communicator

uavcan_communicator — это мост между UAVCAN и ROS. Он обеспечивает минимально необходимый набор датчиков для UAVCAN-HITL-симуляции, а также может быть использован и для других целей. В таблицах ниже представлены поддерживаемые преобразования для общего понимания, об обмене какими данными идёт речь:

Да-да, большая часть кода в робототехнике — про преобразование форматов и пересылку сообщений. Вот есть PX4 с собственными uORB внутри — это раз. Всю переферию мы, благо, свели в UAVCAN, но тут тоже свои форматы — это два. Дальше мы цепляем бортовой компьютер (или симулятор, про который идёт речь) c ROS, тут свои сообщения — это три. Ну и когда мы общаемся с наземным пунктом управления принятым стандартом является MAVLINK, тоже свои отдельные форматы сообщений — уже четыре. Наземный пункт управления написан на Qt, там сигналы и слоты. Если используется Gazebo, у него внутри тоже свои топики. Пять, шесть (и это только навскидку) — ну вы поняли, то ещё «перекладывание джсонов».

Хватает ли пропускной способности CAN-шины?

Для начала необходимо проанализировать ограничение скорости. В нашем случае мы используем CAN-сниффер на базе SLCAN для обмена данными между PX4 на CAN-порту со скоростью 1000 Кбит/с и ПК через последовательный порт с той же скоростью. Это и является ограничением. Посмотрим, сколько данных нужно передавать.

Каждый CAN-кадр содержит 12 байт полезных данных (4 байта ID и 8 байт данных). Хотя базовый не расширенный CAN-кадр имеет в худшем случае длину 134 бита, SLCAN всегда преобразует эти данные в 27 байт, где каждый байт имеет дополнительные стартовые и стоповые биты, поэтому мы можем считать его узким местом нашего потока данных. Учитывая вышесказанное, положим, что производительность SLCAN составляет 100 000 байт/с или 3703 CAN-кадра в секунду. В таблице ниже вы можете увидеть все датчики, участвующие в коммуникации, сколько CAN-кадров им требуется для передачи сообщения, их частоту и процент от максимального объема передаваемых данных.

UAVCAN IMU

Хотя автопилот PX4 имеет широкий набор реализованных UAVCAN v0 датчиков, для закрытия необходимого для моделирования минимального набора датчиков UAVCAN нам потребуются дополнительные модули гироскопа и акселерометра. Поскольку PX4 разделяет датчики и аппаратную логику, достаточно легко создать наследников от UavcanSensorBridgeBase, которые подписываются на выбранный стандартный тип данных uavcan.equipment.ahrs.RawIMU и обновляет состояние PX4Accelerometer и PX4Gyroscope. Соответствующий принятый PR в PX4: https://github.com/PX4/PX4-Autopilot/pull/17026.

Итак, из таблицы видно, что в настоящий момент датчики (без IMU) потребляют только 15,3% потока, поэтому мы можем легко использовать IMU с частотой не менее 200 Гц, и он будет потреблять дополнительные 38% потока. Другой вопрос заключается в загрузке CPU, когда мы используем UAVCAN IMU вместо I²C/SPI. Наши тесты на CUAV v5+ в режиме ожидания с активированными датчиками показали, что UAVCAN IMU с частотой 200 Гц потребляет меньше CPU, чем стандартный внутренний IMU на шинах I²C/SPI: 43,4% против 64,8%. В то же время следует отметить, что внутренние IMU представлены 3 комбинированными гироскопами и акселерометрами (icm20602, icm20689 и bmi055), которые работают на частоте 400 Гц. Производительность можно было бы значительно увеличить, улучшив CAN-адаптер или даже используя CAN-FD вместо стандартного CAN, но поскольку мы пока не используем UAVCAN IMU в реальном полете, а для моделирования достаточно частоты 200 Гц, этот вопрос оставляем за скобками.

inno_sim_interface

Для трёхмерной реалистичной визуализации, симуляции камер и лидаров используем интерфейс подключения к Innopolis simulator: inno_sim_interface.

Как тестануть?

Проект довольно нишевый, предназначен для разработчиков, работающих с PX4 и использующих (или планирующих использовать) UAVCAN. В связи с этим важно любое ваше участие. Скачивайте, пробуйте, пишите Issues и комментарии, присылайте PR-ы. Далее краткая инструкция, какие есть варианты запуска, и что для этого нужно, со ссылками на readme.

Вариант True HITL

• Нужно из железа:

  • PX4 — мы используем и рекомендуем CUAV V5+;

  • USB-UAVCAN — у нас есть свои, обращайтесь, если нужно. Подойдут, думаю, и от OlliW и Zubax и даже Kvazer;

  • компьютер с Ubuntu (мы тестировали на 18.04, на 20-й тоже должно работать) и ROS (melodic — проверено или noetic — должно завестись). Для запуска 3d-визуализатора есть доп. требования по производительности компьютера;

• Клоним и билдим по инструкции https://github.com/InnopolisAero/innopolis_vtol_dynamics#2-installation-and-building;

• Прошиваем PX4 (нашим Firmware);

• Запускаем https://github.com/InnopolisAero/innopolis_vtol_dynamics#3-usage-example.

Вариант SITL — просто пощупать

Тут гораздо проще — вам не нужно ничего кроме компьютера, см. инструкцию и пропускайте всё, что касается HITL, а что касается SITL, соответсвенно, запускайте. Так получится потестить динамику и 3d-визуализацию.

Profit

Уже на этапе тестирования до того как мы опубликовали проект у нас появился кейс отлавливания бага в прошивке PX4. @PonomarevDA заметил при работе с новой прошивкой, что после нескольких минут использования UAVCAN-датчика дифференциального давления (симулируемого или реального), его фильтрованное значение всегда становится нестабильным. innopolis_vtol_dynamics стал удобным инструментом чтобы тестить фиксы.

Credits

Команда RaccoonLab благодарит вас за внимание!
Наш инстаграмчик: https://www.instagram.com/raccoon.lab/, велком. Мы ценим ваши лайки и подписки.
О том, что у нас еще есть по теме (в том числе девайсы): http://raccoonlab.org/uavcan.

Авторы проекта:

• Дмитрий Пономарёв @PonomarevDA — большая часть реализации за ним и плюсы ему, плиз;

• Константин Бурдинов @sainquake и Айрат Биляев — электроника;

• Андрей Маевский — аэродинамика;

• Илья Тарасов — моделирование;

• Дмитирий Девитт @GigaFlopsis — лётные тесты;

• Роман Федоренко — втянул всех в эту движуху и написал статейку.

Благодарности:

• Сергею Копылову и Top dev team за виртуальный Иннополис и за то, что там есть наши БПЛА;

• Павлу Кириенко @Spym за создание UAVCAN, развитие сообщества и моральную поддержку наших проектов.

Pixhawk (также PX4FMU) — проект по созданию открытого аппаратного обеспечения для полетного контроллера квадрокоптеров и других БПЛА.
Полетные контроллеры Pixhawk содержат следующий набор оборудования:

• Микроконтроллер на базе ядра ARM Cortex M4, например, STM32F427 на Pixhawk 1.
• Дополнительный микроконтроллер, предусмотренный на случай отказа основного.
• Гироскоп, акселерометр, магнитометр.
• Барометр.
• Яркий светодиод, который может сигнализировать о текущем состоянии, режиме и неполадках контроллера.

Также возможно подключение дополнительной периферии через интерфейсы UART, CAN, I²C, SPI и другие.

Периферийные устройства контроллера полета:

1. GPS и компас.

PX4 поддерживает глобальные навигационные спутниковые системы (GNSS) (включая GPS, ГЛОНАСС, Galileo, BeiDou, QZSS и SBAS) с использованием приемников, которые обмениваются данными по протоколам u-blox, MTK Ashtech или Emlid или через UAVCAN. Он также поддерживает кинематические GPS-приемники реального времени (RTK), которые расширяют возможности GPS-систем до сантиметровой точности.

PX4 можно использовать со следующими компонентами компаса (магнитометрами): Bosch BMM 150 MEMS (через шину I2C), HMC5883 / HMC5983 (I2C или SPI), IST8310 (I2C) и LIS3MDL (I2C или SPI). Можно подключить до 4 внутренних или внешних магнитометров, хотя фактически только один будет использоваться в качестве источника курса.

** Настройка оборудования.**

Инструкции по подключению GPS (и компаса, если он есть) обычно предоставляются производителем. Контроллеры серии Pixhawk обычно имеют четко обозначенный порт для подключения GPS, а компас подключается либо к порту/шине I2C, либо к SPI (в зависимости от устройства). ARK GPS, ARK RTK GPS, Zubax GNSS 2, CubePilot Here3 CAN GNSS GPS (M8N) и Avionics Anonymous GNSS/Mag также можно подключить через UAVCAN.

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

Конфигурация GNSS
«Стандартная» конфигурация GPS представлена ​​ниже. Дополнительная конфигурация конкретного устройства может быть предоставлена ​​в PX4 или документации производителя устройства (например, Trimble MB-Two > Configuration).

Протокол GPS, ожидаемый PX4, по умолчанию имеет значение u-blox (по умолчанию другие типы GPS, такие как Trimble, Emlid, MTK, не будут обнаружены). Протокол можно настроить с помощью GPS_x_PROTOCOL.

Настройка основного GPS
Конфигурация GPS на Pixhawk выполняется прозрачно для пользователя — просто подключите модуль GPS к порту с маркировкой GPS , и все должно работать.

Конфигурация последовательного порта по умолчанию работает для большинства устройств. Если вы используете Trimble MB-Two , вам потребуется изменить конфигурацию, чтобы явно установить скорость 115200 бод.

Настройка вторичного GPS (двойная система GPS).
Чтобы использовать дополнительный GPS, подключите его к любому свободному порту, а затем выполните настройку последовательного порта , чтобы назначить GPS_2_CONFIG для выбранного порта.

Следующие шаги показывают, как настроить дополнительный GPS на TELEM 2 порту в QGroundControl:

1. Найдите и установите для параметра GPS_2_CONFIG значение TELEM 2.
   • Откройте QGroundControl и перейдите в раздел «Настройка транспортного средства (ТС)» > «Параметры».
   • Выберите вкладку GPS (1), затем откройте параметр GPS_2_CONFIG (2) и выберите TELEM 2 из выпадающего списка (3).

1. Перезагрузите ТС, чтобы другие параметры стали видны.
2. Выберите вкладку Serial и откройте параметр SER_TEL2_BAUD ( TELEM 2 скорость передачи порта): установите для него значение Auto.

После настройки второго порта GPS:

Настройте средство оценки ECL/EKF2 для смешивания данных из обеих систем GPS. Подробные инструкции см. в разделе Использование ECL EKF > Двойные приемники.

Настройка GPS в качестве источника курса

При использовании GPS в качестве источника курса вам необходимо настроить следующие параметры:

2. Телеметрические радиостанции/модемы.

Радиостанции телеметрии могут (опционально) использоваться для обеспечения беспроводного соединения MAVLink между наземной станцией управления, такой как QGroundControl, и транспортным средством, работающим под управлением PX4. Это позволяет настраивать параметры во время полета аппарата, проверять телеметрию в режиме реального времени, менять миссию на лету и т.д.

PX4 поддерживает несколько типов телеметрических радиостанций:

1. Прошивка на основе SiK Radio (в общем случае должно работать любое радио с интерфейсом UART).
   • Радиотелеметрия RFD900.
   • Телеметрическое радио HolyBro SiK
2. Телеметрия Wi-Fi.
3. Последовательная телеметрическая радиосвязь Microhard.
   • Последовательная телеметрическая радиостанция ARK Electron Microhard.
   • Телеметрическая радиостанция Holybro Microhard P900.
4. Последовательная телеметрическая радиосвязь XBee.
   • Радиотелеметрия HolyBro XBP9X.
5. Серийная телеметрическая радиостанция CUAV.
   • Радиотелеметрия CUAV P8.

Протокол PX4 совместим с SiK Radio и, как правило, работает сразу после распаковки (хотя вам может потребоваться изменить/использовать соответствующий разъем).

Телеметрия Wi-Fi обычно имеет меньший радиус действия, более высокую скорость передачи данных и упрощает поддержку FPV/видеопотоков. Одним из преимуществ WiFi-радиостанций является то, что вам нужно приобрести только один радиомодуль для вашего ТС (при условии, что наземная станция уже имеет WiFi).

PX4 не поддерживает подключение USB-модуля LTE к полетному контроллеру (и отправку трафика MAVLink через Интернет). Однако вы можете подключить модуль LTE к компьютеру-компаньону и использовать его для маршрутизации трафика MAVLink с полетного контроллера. Дополнительные сведения см. в разделе: Компьютерные периферийные устройства-компаньоны > Телефония для передачи данных.

3. Датчики расстояния (дальномеры).

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

Поддерживаемые дальномеры:

• ARK Flow — это модуль датчика времени пролета (ToF) и оптического потока с открытым исходным кодом, который способен измерять расстояния от 8 см до 30 м. Его можно подключить к полетному контроллеру через порт CAN1, что позволяет подключать дополнительные датчики через порт CAN2. Он работает с прошивкой PX4, поддерживает обновление прошивки UAVCAN и упакован в крошечный форм-фактор.

• VL53L1X — это современный Time-of-Flight (ToF) лазерный датчик дальности, расширяющий семейство продуктов ST FlightSense™. Это самый быстрый миниатюрный датчик ToF на рынке с точным диапазоном до 4 м и быстрой частотой до 50 Гц. Он поставляется с 4-контактным разъемом JST GHR, который совместим с портом I2C на Pixhawk 4 , Pixhawk 5X и других полетных контроллерах, соответствующих стандарту разъема Pixhawk.

• Lidar-Lite — это компактный высокопроизводительный оптический дальномер. Он имеет диапазон датчика от 5 см до 40 м и может быть подключен к портам PWM или I2C.

• MaxBotix I2CXL-MaxSonar-EZ — имеет ряд дальномеров на основе сонара относительно малого радиуса действия, которые подходят для вспомогательного взлета / посадки и предотвращения столкновений. Их можно подключить через порт I2C.

и т.д.

MaxBotix I2CXL-MaxSonar-EZ

Дальномеры включаются с помощью параметра SENS_EN_MB12XX.

Линейка I2CXL-MaxSonar-EZ позволяет использовать до 127 датчиков на одной шине через интерфейс I2C. Эти ультразвуковые дальномеры предлагают разрешение 1 см, работу от 3,0 до 5,5 В постоянного тока, высокую выходную мощность, подавление шума и автоматическую калибровку. Полные технические характеристики см. в техническом описании.

Распиновка I2CXL-MaxSonar-EZ

• GND: Возврат для источника питания постоянного тока. GND (и V+) должен быть свободным от пульсаций и помех для лучшей работы.
• V+: Работает от 3 В постоянного тока до 5,5 В постоянного тока. Среднее потребление тока при работе с напряжением 3,3 В составляет 2,7 мА (пиковое значение 50 мА), а при напряжении 5 В рабочий ток составляет 4,4 мА (пиковое значение 100 мА) соответственно. Пиковый ток используется во время передачи импульса сонара.
• Pin 5-SCL (I2C Clock): Это линия синхронизации для связи I2C. Эти датчики поддерживают тактовые частоты I2C до 400 кГц при условии, что ведущее устройство поддерживает растяжку тактовой частоты. Без растягивания часов устройства могут работать на скорость до 50кГц.
• Pin 4-SDA (I2C Data): Это линия данных для связи I2C. Эти датчики работают как ведомые устройства I2C.
• Pin 3-Not Used: Этот пин не используется.
• Pin 2-Address Announce / Status: Пока датчик выполняет считывание диапазона, на этом выводе устанавливается высокий уровень, а связь I2C игнорируется. Во время работы без дальности этот вывод удерживается низким, и датчик прослушивает входящие сообщения I2C. При желании пользователи могут опрашивать этот контакт, чтобы определить, завершил ли датчик свой цикл измерения дальности и готов ли он сообщить последнюю информацию о дальности.
  Во время включения питания этот вывод будет обеспечивать представление ширины импульса текущего адреса датчика с длиной ~ 100 микросекунд на цифру. (Адрес по умолчанию 224 будет объявляться импульсом длиной 22 400 микросекунд).
• Pin 1-Temporary Default: При включении проверяется состояние этого вывода; если оставить высокий уровень или отключить, датчик будет использовать адрес, хранящийся в памяти, для связи I2C. При низком уровне датчик будет использовать свой адрес по умолчанию для текущего цикла питания.

Контакты I2C A
Распиновка портов PX4

Конфигурация/настройка

Дальномеры обычно подключаются либо к последовательному (PWM), либо к порту I2C (в зависимости от драйвера устройства) и включаются на порте путем установки определенного параметра.

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

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

Общая конфигурация

Общая конфигурация дальномера задается параметрами EKF2_RNG_* . К ним относятся:

• EKF2_RNG_POS_X, EKF2_RNG_POS_Y, EKF2_RNG_POS_Z — смещение дальномера от центра тяжести ТС по осям X, Y, Z.

• EKF2_RNG_PITCH — значение 0 градусов (по умолчанию) соответствует дальномеру, который точно выровнен с вертикальной осью ТС (т. е. прямо вниз), а 90 градусов указывает, что дальномер направлен вперед. Простая тригонометрия используется для расчета расстояния до земли, если используется ненулевой шаг.

• EKF2_RNG_DELAY — примерная задержка поступления данных от датчика на оценщик.

• EKF2_RNG_SFE — масштабатор шума, зависящий от дальности дальномера.

• EKF2_RNG_NOISE — шум измерения для слияния дальномера.

параметры прописываются: PX4-Autopilot/ROMFS/px4fmu_common/init.d/airframes

Изменение параметра.

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

Когда вы нажимаете «Сохранить», параметр автоматически загружается в подключенное транспортное средство. В зависимости от параметра вам может потребоваться перезагрузить контроллер полета, чтобы изменения вступили в силу.

Инструменты.

Вы можете выбрать дополнительные параметры в меню «Инструменты» в верхней правой части экрана.

• Refresh — jбновить значения параметров, повторно запросив их все у ТС.

• Reset all to defaults — cбросить все параметры к исходным значениям по умолчанию.

• Load from/Save to file … — загрузить параметры из существующего файла или сохранить текущие настройки параметров в файл.

• Clear RC to Param — This clears all associations between RC transmitter controls and parameters. For more information see: Настройка радио > Каналы настройки параметров.

• Reboot Vehicle — перезагрузить ТС (требуется после изменения некоторых параметров).

Тестирование.

Для тестирования дальномера необходимо включить модуль distance_sensor в состав прошивки (mb12xx).

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

QGroundControl MAVLink Inspector

QGroundControl MAVLink Inspector позволяет просматривать сообщения, отправленные с ТС, включая DISTANCE_SENSOR информацию с дальномера. Основное различие между инструментами заключается в том, что инструмент «Анализ» может отображать значения в виде графика.

Примечание

Отправляемые сообщения зависят от конфигурации ТС. Вы будете получать DISTANCE_SENSOR сообщения только в том случае, если на подключенном ТС установлен дальномер и он публикует значения датчиков.

Для просмотра выходных данных дальномера:

1. Откройте меню Q > Select Tool > Analyze Tools:

1. Выберите сообщение DISTANCE_SENSOR и установите флажок графика против current_distance. Затем инструмент нанесет результат:

Консоль QGroundControl MAVLink

listener distance_sensor 5

Примечание

Консоль QGroundControl MAVLink работает при подключении к Pixhawk или другим целям NuttX, но не к симулятору. В Симуляторе вы можете запускать команды прямо в терминале.

Моделирование.

4. Тахометры (счетчики оборотов).

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

В этом разделе перечислены датчики тахометра, поддерживаемые PX4. Более подробная информация об установке и настройке представлена ​​в разделах, указанных ниже.

Поддерживаемое оборудование

• ThunderFly TFRPM01 Тахометр — это небольшая и низкая система, требующая счетчика оборотов. Сама плата не включает в себя фактический датчик, но может использоваться со многими различными типами датчиков/щупов для подсчета оборотов. Он имеет разъем I²C для подключения к PX4 и подключается к датчику через 3-контактный разъем. Он также имеет светодиод, который предлагает основную диагностическую информацию.

Датчик TFRPM01 представляет собой аппаратное обеспечение с открытым исходным кодом, которое можно приобрести в компании ThunderFly sro.(данные о производстве доступны на GitHub).

Настройка оборудования

Плата оснащена (двумя проходными) разъемами I²C для подключения к PX4 и имеет 3-контактный разъем, который можно использовать для подключения к различным датчикам:

1. TFRPM01 можно подключить к любому порту I²C.
2. TFRPM01 имеет 3-контактный штекерный разъем (со входом с подтяжкой), который можно подключать к датчикам различных типов:
   • Аппаратному обеспечению датчика/зонда требуется импульсный сигнал. Входной сигнал принимает логику +5 В TTL или открытый коллектор выходы. Максимальная частота импульсов составляет 20 кГц при коэффициенте заполнения 50%.
   • Разъем пробника обеспечивает питание +5 В от шины I²C, максимальная мощность, которую можно использовать, ограничена RC-фильтром (подробности см. на схемах).

Электроника TFRPM01A оснащена сигнальным светодиодом, который можно использовать для проверки правильности подключения зонда. Светодиод загорается, когда импульсный вход заземлен или подвергается воздействию логического 0, поэтому вы можете проверить правильность работы пробника, просто вращая ротор вручную.

Зонд датчика Холла

Датчики Холла (с магнитным приводом) идеально подходят для суровых условий, когда грязь, пыль и вода могут контактировать с датчиком ротора.

Оптический сенсорный датчик

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

Настройка программного обеспечения

Начальный драйвер

Драйвер не запускается автоматически (в любом планере). Вам нужно будет запустить его вручную, либо с помощью консоли QGroundControl MAVLink или добавив драйвер в сценарий запуска на SD-карте.

Запустите драйвер из консоли с помощью команды:

pcf8583 start -X -b <bus number>

где:

• -X означает, что это внешняя шина.
• <bus number> номер шины, к которой подключено устройство.

5. Камера.

6. Оптический поток.

Способы подключения периферийного оборудования:

Обзор схемы подключения

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

GPS + компас + зуммер + предохранительный выключатель + светодиод

Подключите прилагаемый GPS со встроенным компасом, защитным выключателем, зуммером и светодиодом к порту GPS MODULE.

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

Power

Подключите выход платы управления питанием (PM board), которая входит в комплект, к одному из модулей POWER Pixhawk 4 с помощью 6-жильного кабеля. Вход PM 2~12S будет подключен к вашей батарее LiPo. Соединения платы управления питанием, включая подключение питания и сигналов к регуляторам скорости и сервоприводам, поясняются в таблице данного раздела. Обратите внимание, что плата PM не подает питание на сервоприводы через + и — контакты FMU PWM-OUT.

На изображении ниже показана плата управления питанием, поставляемая с Pixhawk 4.

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

Радиосистема дистанционного управления (RC) требуется, если вы хотите вручную управлять своим транспортным средством (PX4 не требует радиосистемы для автономных режимов полета).

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

В приведенных ниже инструкциях показано, как подключить различные типы приемников к Pixhawk 4:

• Приемники Spektrum/DSM или S.BUS подключаются к входу DSM/SBUS RC.

• Приемники PPM подключаются к входному порту PPM RC.

• Приемники PPM и PWM, которые имеют отдельный провод для каждого канала, должны подключаться к порту PPM RC через кодировщик PPM, подобный этому (приемники PPM-Sum используют один сигнальный провод для всех каналов).

Телеметрические радиостанции

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

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

Моторы

Двигатели/сервоприводы подключаются к портам I/O PWM OUT ( MAIN ) и FMU PWM OUT ( AUX ) в порядке, указанном для вашего ТС в Справочнике по планеру.

Обзор шины I2C

I²C — последовательная асимметричная шина для связи между интегральными схемами внутри электронных приборов. Использует две двунаправленные линии связи (SDA и SCL), применяется для соединения низкоскоростных периферийных компонентов с процессорами и микроконтроллерами (например, на материнских платах, во встраиваемых системах, в мобильных телефонах).

Pixhawk/PX4 поддерживает его для:

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

• Совместимость с периферийными устройствами, поддерживающими только I2C.

• Разрешение нескольким устройствам подключаться к одной шине (полезно для экономии портов). Например, светодиоды, компас, дальномеры и т. д.

I2C позволяет нескольким ведущим устройствам подключаться к нескольким подчиненным устройствам, используя только 2 провода для каждого соединения (SDA, SCL). Теоретически шина может поддерживать 128 устройств, доступ к каждому из которых осуществляется по уникальному адресу.

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

Интеграция устройств I2C

Драйверы должны включать #include <drivers/device/i2c.h>, затем предоставить реализацию абстрактного базового класса I2C определенного в I2C.hpp для целевого оборудования (т.е. для NuttX).

Ниже приведены основные методы реализованные в I2C.сpp и необходимые для работы с шиной I2C:

 int I2C::init()
 {
    int ret = PX4_ERROR;
unsigned bus_index;

// attach to the i2c bus
_dev = px4_i2cbus_initialize(get_device_bus());

if (_dev == nullptr) {
	DEVICE_DEBUG("failed to init I2C");
	ret = -ENOENT;
	goto out;
}

// the above call fails for a non-existing bus index,
// so the index math here is safe.
bus_index = get_device_bus() - 1;

// abort if the max frequency we allow (the frequency we ask)
// is smaller than the bus frequency
if (_bus_clocks[bus_index] > _frequency) {
	(void)px4_i2cbus_uninitialize(_dev);
	_dev = nullptr;
	DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %" PRIu32 " KHz)",
		   get_device_bus(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
	ret = -EINVAL;
	goto out;
}

// set frequency for this instance once to the bus speed
// the bus speed is the maximum supported by all devices on the bus,
// as we have to prioritize performance over compatibility.
// If a new device requires a lower clock speed, this has to be
// manually set via "fmu i2c <bus> <clock>" before starting any
// drivers.
// This is necessary as automatically lowering the bus speed
// for maximum compatibility could induce timing issues on
// critical sensors the adopter might be unaware of.

// set the bus frequency on the first access if it has
// not been set yet
if (_bus_clocks[bus_index] == 0) {
	_bus_clocks[bus_index] = _frequency;
}

// call the probe function to check whether the device is present
ret = probe();

if (ret != OK) {
	DEVICE_DEBUG("probe failed");
	goto out;
}

// do base class init, which will create device node, etc
ret = CDev::init();

if (ret != OK) {
	DEVICE_DEBUG("cdev init failed");
	goto out;
}

// tell the world where we are
DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %" PRIu32 " KHz)",
	     get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
out:


    if ((ret != OK) && (_dev != nullptr)) {
	px4_i2cbus_uninitialize(_dev);
	_dev = nullptr;
}

return ret;}

transfer выполняет транзакцию I2C на устройстве. По крайней мере один из send_len и recv_len должен быть ненулевым.

@param send — указатель на байты для отправки.

@param send_len — количество байтов для отправки.

@param recv — указатель на буфер для полученных байтов.

@param recv_len — количество байтов для получения.

@return OK, если передача прошла успешно, -errno в противном случае.

 int I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
 {

int ret = PX4_ERROR;
unsigned retry_count = 0;

if (_dev == nullptr) {
	PX4_ERR("I2C device not opened");
	return PX4_ERROR;
}

do {
	DEVICE_DEBUG("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);

	i2c_msg_s msgv[2] {};
	unsigned msgs = 0;

	if (send_len > 0) {
		msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
		msgv[msgs].addr = get_device_address();
		msgv[msgs].flags = 0;
		msgv[msgs].buffer = const_cast<uint8_t *>(send);
		msgv[msgs].length = send_len;
		msgs++;
	}

	if (recv_len > 0) {
		msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
		msgv[msgs].addr = get_device_address();
		msgv[msgs].flags = I2C_M_READ;
		msgv[msgs].buffer = recv;
		msgv[msgs].length = recv_len;
		msgs++;
	}

	if (msgs == 0) {
		return -EINVAL;
	}

	int ret_transfer = I2C_TRANSFER(_dev, &msgv[0], msgs);

	if (ret_transfer != 0) {
		DEVICE_DEBUG("I2C transfer failed, result %d", ret_transfer);
		ret = PX4_ERROR;

	} else {
		// success
		ret = PX4_OK;
		break;
	}

	/* if we have already retried once, or we are going to give up, then reset the bus */
	if ((retry_count >= 1) || (retry_count >= _retries)) {
            #if defined(CONFIG_I2C_RESET)

		I2C_RESET(_dev);

             #endif // CONFIG_I2C_RESET

	}

} while (retry_count++ < _retries);

return ret;}

Драйверы также должны включать заголовки для своего типа устройства (drv_*.h) в /src/drivers/- например, drv_baro.h .

Далее представлены основные этапы написания драйвера, на примере драйвера mb12xx.

Конфигурационные константы:

   #define MB12XX_BASE_ADDR                        0x70   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
   #define MB12XX_MIN_ADDR                         0x5A   // 7-bit address is 0x5A = 90.  8-bit address is 0xB4 = 180.
   #define MB12XX_BUS_SPEED                        100000 // 100kHz bus speed.

Регистрация адресов:

   #define MB12XX_TAKE_RANGE_REG                   0x51 // Measure range Register.
   #define MB12XX_SET_ADDRESS_1                    0xAA // Change address 1 Register.
   #define MB12XX_SET_ADDRESS_2                    0xA5 // Change address 2 Register.

Ограничения по устройствам:

   #define MB12XX_MEASURE_INTERVAL                 100_ms // 60ms minimum for one sonar.
   #define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!)

Класс MB12XX является публичным наследником класса device::I2C.

Вывод основной информации о драйвере:

   void print_status() override;

Устанавливает новый адрес устройства.

@param address — новый адрес датчика, который необходимо установить: 200-224 только четные адреса.

@return возвращает PX4_OK в случае успеха, PX4_ERROR в противном случае.

   int  set_address ( const  uint8_t address = MB12XX_BASE_ADDR);

Собирает самые последние данные измерений датчиков с шины i2c.

   int collect();

   int _measure_interval{MB12XX_MEASURE_INTERVAL};	// Initialize the measure interval for a single sensor.
   int _sensor_index{0};	// Initialize counter for cycling i2c adresses to zero.
   int _sensor_count{0};

Передача данных с шины:

int ret_val = transfer(nullptr, 0, &val[0], 2);

Чтобы включить драйвер в прошивку, вы должны добавить драйвер в файл cmake для конкретной платы, который соответствует цели, для которой вы хотите выполнить сборку:

drivers/sf1xx

Например, вы можете посмотреть/найти этот драйвер в файле px4_fmu-v4_default конфигурации.

Примеры драйверов I2C

Чтобы найти примеры драйверов I2C, найдите i2c.h в /src/drivers/.

Вот несколько примеров:

• drivers/sf1xx- Драйвер I2C для Lightware SF1XX LIDAR.
• drivers/ms5611- Драйвер I2C для датчиков барометрического давления MS5611 и MS6507, подключенных через I2C (или SPI).

Принцип подключения

Шина I2C синхронная, состоит из двух линий: данных (SDA) и тактирования (SCL). Есть ведущий (master) и ведомые (slave). Инициатором обмена всегда выступает ведущий, обмен между двумя ведомыми невозможен.

Такты на линии SCL генерирует master. Линией SDA могут управлять как мастер, так и ведомый в зависимости от направления передачи. Единицей обмена информации является пакет, обрамленный уникальными условиями на шине, именуемыми стартовым и стоповым условиями. Мастер в начале каждого пакета передает один байт, где указывает адрес ведомого и направление передачи последующих данных. Данные передаются 8-битными словами. После каждого слова передается один бит подтверждения приема приемной стороной.

Принцип работы

I²C использует две двунаправленные линии, подтянутые к напряжению питания и управляемые через открытый коллектор или открытый сток — последовательная линия данных (SDA, англ. Serial DAta) и последовательная линия тактирования (SCL, англ. Serial CLock). Стандартные напряжения +5 В или +3,3 В, однако допускаются и другие.

Классическая адресация включает 7-битное адресное пространство с 16 зарезервированными адресами. Это означает, что разработчикам доступно до 112 свободных адресов для подключения периферии на одну шину.

Основной режим работы — 100 кбит/с; 10 кбит/с в режиме работы с пониженной скоростью. Также немаловажно, что стандарт допускает приостановку тактирования для работы с медленными устройствами.

Wiring

I²C — шина с открытым стоком, что означает, что устройства заземляют линию данных. Он использует подтягивающий резистор, чтобы подтолкнуть его к лог.1 (состояние ожидания). Одна шина может подключаться к нескольким устройствам I2C. Отдельные устройства подключаются без пересечения.

Для подключения (по стандарту dronecode) используются 4-жильные кабели, оснащенные разъемами JST-GH. Для обеспечения надежной связи и уменьшения перекрестных помех рекомендуется применять рекомендации по скручиванию кабеля и размещению подтягивающих резисторов.

Кабели I²C

Цвета кабелей от нескольких поставщиков указаны в следующей таблице.

Стандарт Dronecode предполагает использование подтягивающего резистора 1,5 кОм для сигналов SDA и SCL в автопилоте.

Скручивание кабеля

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

• 10 витков на каждую пару SCL/+5V и SDA/GND на 30 см длины кабеля.

• 4 витка обеих пар вместе на 30 см длины кабеля.

Проверка состояния шины и устройства

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

Инструмент можно запустить в терминале PX4 с помощью следующей команды:

i2cdetect -b 1

где номер шины указывается после параметра -b

Шина I2C (обзор разработки)