Разбираемся в MAVLink. Часть 1

2016-10-11 23:10

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

MAVLink или Micro Air Vehicle Link - это протокол информационного взаимодействия с дронами или малыми беспилотными аппаратами (летающими, плавающими, ползающими и т.д), далее по тексту называемых MAV (Micro Air Vehicle). MAVLink распространяется под LGPL лицензией в виде модуля для python (есть удобная обёртка DroneKit) и генератора header-only С/C++ библиотеки. Есть так же репозитории уже сгенерированных библиотек для MAVLink версии v1 (этой мы и будем пользоваться) и версии v2.

Протокол описывает информационное взаимодействие между системами, такими как MAV и GCS(Ground control station) - станция наземного управления, а так же их составными частями - компонентами. Базовой сущностью MAVLink является пакет, имеющий следующий формат:

Первый байт пакета (STX) - это символ начала сообщения: 0xFD для версии v2.0, 0xFE для версии v1.0, 0x55 для версии v0.9. LEN - длинна полезной нагрузки (сообщения). SEQ - содержит счётчик пакета (0-255), который поможет нам выявить потерю сообщения. SYS (System ID) - идентификатор отправляющий системы, а COMP (Component ID) - идентификатор отправляющего компонента. MSG (Message ID) - тип сообщения, от него зависит, какие данные будут лежать в полезной нагрузки пакета. PAYLOAD - полезная нагрузка пакета, сообщение, размером от 0 до 255 байт. Два последних байта пакета - CKA и CKB, нижний и верхний байт, соответственно, содержат контрольную сумму пакета.

Библиотека MAVLink позволяет кодировать и раскодировать пакеты согласно протоколу, но она не регламентирует, какими аппаратными и программными средствами данные будет отправлены - это могут быть TCP/UDP сообщения, обмен через последовательный порт, да что угодно, что обеспечивает двухсторонний обмен. Библиотека обрабатывает входные данные побайтово, добавляя их в буфер и сама собирает из них пакет. Каждая система или компонент, может одновременно обмениваться данными по разным источникам, тогда для каждого источника назначается специальный идентификатор, называемый channel (канал). MAVLink содержит буфер на каждый канал.

Получаем heartbeat с борта

Перейдём от теории к практике и попробуем написать ООП-обёртку поверх MAVLink. Ниже, я буду приводить примеры кода на C++ с использованием Qt. Я выбрал этот инструмент, во-первых, потому, что в будущем планирую визуализировать некоторые параметры MAVLink с использованием Qt Quick и Qt Location, а во-вторых, многие решения я подсмотрел в проекте qgroundcontrol, так же написанного на Qt.

Для начала, введём абстракцию над каналом связи, пусть это будет класс AbstractLink, в его интерфейсе определим функциональность, позволяющую нам получать и отправлять данные в виде QByteArray. Наследники этого класса, UdpLink и SerialLink, обеспечат нам передачу данных по сети и через последовательный порт.

Интерфейс класса AbstractLink

class AbstractLink: public QObject {     Q_OBJECT      public:     explicit AbstractLink(QObject* parent = nullptr);          virtual bool isUp() const = 0;      public slots:     virtual void up() = 0;     virtual void down() = 0;          virtual void sendData(const QByteArray& data) = 0;      signals:     void upChanged(bool isUp);     void dataReceived(const QByteArray& data); };

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

Интерфейс класса MavLinkCommunicator

class MavLinkCommunicator: public QObject {     Q_OBJECT      public:     MavLinkCommunicator(QObject* parent = nullptr);      public slots:     void addLink(AbstractLink* link, uint8_t channel);     void removeLink(AbstractLink* link);          void sendMessage(mavlink_message_t& message, AbstractLink* link);     void sendMessageOnLastReceivedLink(mavlink_message_t& message);     void sendMessageOnAllLinks(mavlink_message_t& message);      signals:     void messageReceived(const mavlink_message_t& message);      private slots:     void onDataReceived(const QByteArray& data);      private:     QMap<AbstractLink*, uint8_t> m_linkChannels;     AbstractLink* m_lastReceivedLink; };

Рассмотрим, как осуществляется сборка пакета из потока данных. Как было сказано выше, MAVLink читает входящий поток данных побайтово, для этого используется функция mavlink_parse_char, которая возвращает данные сообщения или NULL, если сообщение не может быть получено, сохраняя полученный символ во внутренний буфер. MAVLink содержит буфер для каждого канала. Такой подход позволяет передавать данные с последовательного порта напрямую в функцию разбора пакета MAVLink, лишая пользователя библиотеки удовольствия вручную собирать сообщения из потока.

Метод сборки пакета класса MavLinkCommunicator

void MavLinkCommunicator::onDataReceived(const QByteArray& data) {     mavlink_message_t message;     mavlink_status_t status;      // onDataReceived это слот, который вызываться строго по сигналу от AbstractLink     m_lastReceivedLink = qobject_cast<AbstractLink*>(this->sender());      if (!m_lastReceivedLink) return;      // идентификатор канала получаем из словаря     uint8_t channel = m_linkChannels.value(m_lastReceivedLink);      for (int pos = 0; pos < data.length(); ++pos)     {         if (!mavlink_parse_char(channel, (uint8_t)data[pos],                                 &message, &status))             continue;          emit messageReceived(message); // по этому сигналу происходит обработка принятого пакета     }     // Обработка статуса канала связи }

Для получения полезных данных одной только сборки пакета мало. Необходимо получить из пакета сообщение, извлечь полезную нагрузку согласно идентификатору msgid. MAVLink имеет набор встроенных типов, под каждый msgid (тип сообщения) и функции получения этих сообщений из пакета. Введём ещё один абстрактный тип - AbstractHandler, в интерфейсе этого класса определим чисто виртуальный слот processMessage для обработки сообщения, полученного от MavLinkCommunicator'а. Наследники класса AbstractHandler будут решать, могут ли они обработать то или иное сообщение и, по-возможности, обрабатывать. К примеру, мы хотим обрабатывать сообщения типа heartbeat. Этот самый базовый пакет, в котором система говорит, что она существуют, и что оно такое. Стоит заметить, что heartbeat - это единственный тип пакета, который MAVLink обязывает к использованию. Введём обработчик сообщений этого типа - HeartbeatHandler.

Реализация метода processMessage класса HeartbeatHandler

void HeartbeatHandler::processMessage(const mavlink_message_t& message) {     // проверяем, можем ли обработать пакет     if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT) return;          mavlink_heartbeat_t heartbeat; // создаём сообщение heartbeat     mavlink_msg_heartbeat_decode(&message, &heartbeat); // наполняем его из полученного пакета          // Здесь должна быть обработка сообщения, но у нас пока будет отладочный вывод     qDebug() << "Heartbeat received, system type:" << heartbeat.type;  }

Теперь, если мы настроем классы и установим правильно связь, то сможем получать heartbeat сообщения от полётного контроллера. Я воспользуюсь парой радио модемов и Raspberry Pi с шилдом NAVIO2, на котором запущен автопилот APM. Теоретически, это должно работать с любым автопилотом, поддерживающим текущую версию MAVLink, но если у Вас нет ничего под рукой, чуть дальше будет пример с имитатором автопилота.

код функции main

int main(int argc, char* argv[]) {     QCoreApplication app(argc, argv);      domain::MavLinkCommunicator communicator;      domain::HeartbeatHandler heartbeatHandler; // добавляем обработчик сообщений heartbeat     QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,                      &heartbeatHandler, &domain::HeartbeatHandler::processMessage);      domain::SerialLink link("/dev/ttyUSB0", 57600); // путь к радиомодему и его скорость     communicator.addLink(&link, MAVLINK_COMM_0);     link.up();      return app.exec(); }

Запускаем программу, включаем автопилот и через несколько секунд должно побежать:

Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 5 Heartbeat received, system type: 1 System status: 5

Отправляем свой heartbeat

По задумке, каждая система должна отправлять heartbeat, следовательно, и наша тоже. Начнём с реализации функции отправки пакета класса MavLinkCommunicator. Функция mavlink_msg_to_send_buffer записывает пакет message в буфер для отправки. Предполагается, что на этом этапе все поля пакета, включая длину и контрольную сумму, заполнены корректно.

Метод отправки пакета класса MavLinkCommunicator

void MavLinkCommunicator::sendMessage(mavlink_message_t& message, AbstractLink* link) {     if (!link || !link->isUp()) return;      uint8_t buffer[MAVLINK_MAX_PACKET_LEN];     int lenght = mavlink_msg_to_send_buffer(buffer, &message);      if (!lenght) return;     link->sendData(QByteArray((const char*)buffer, lenght)); }

Теперь, когда у нас есть функция отправки пакета, нам остаётся сформировать сообщение и записать его в пакет. Поручим эту задачу уже существующему классу HeartbeatHandler, а в интерфейс AbstractHandler добавим сигнал отправки сообщения. Функция mavlink_msg_heartbeat_encode записывает сообщение heartbeat в пакет, подобные функции есть для всех встроенных сообщений. Обращу внимание читателя, что в mavlink предусмотрены и дополнительные функции, например mavlink_msg_heartbeat_pack позволяет записать сообщение heartbeat в mavlink_message_t без явного создания объекта типа mavlink_heartbeat_t, а mavlink_msg_heartbeat_send сразу отправляет данные, при наличии определённой функции отправки. Подробнее, как работать с этими функциями, можно ознакомиться по ссылке. Дополнительное окончание _chan (к примеру mavlink_msg_heartbeat_pack_chan) указывает по какому каналу сообщение будет отправлено.

Код события timerEvent класса HeartbeatHandler

void HeartbeatHandler::timerEvent(QTimerEvent* event) {     Q_UNUSED(event)      mavlink_message_t message;     mavlink_heartbeat_t heartbeat;     heartbeat.type = m_type;      mavlink_msg_heartbeat_encode(m_systemId, m_componentId, &message, &heartbeat);      emit sendMessage(message); }

Отправлять heartbeat мы будем по таймеру с частотой 1 Гц. Если поставить отладочный вывод в методе отправки данных канала связи data.toHex(), увидим наши сообщения, согласно приведённой в начале статьи картинке. Каждый такт счётчик должен увеличиваться, а контрольная сумма соответственно меняться.

"fe09000100000821ee85017f0000023f08" "fe09010100000821ee85017f000002d576" "fe09020100000821ee85017f000002ebf5"

Для того, чтобы проверить работает ли наш heartbeat, создадим две цели сборки: gcs - имитатор станции наземного управления и uav - имитатор беспилотника.

код функции main gcs

int main(int argc, char* argv[]) {     QCoreApplication app(argc, argv);      // Для GCS 255 является стандартным sysid     domain::MavLinkCommunicator communicator(255, 0);      // Тип системы - станция наземного управления     domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_GCS);     QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,                      &heartbeatHandler, &domain::HeartbeatHandler::processMessage);     // heartbeat отправляем на все доступные каналы связи      QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage,                      &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks);      // Настройки UDP через localhost     domain::UdpLink link(14550, QString("127.0.0.1"), 14551);     communicator.addLink(&link, MAVLINK_COMM_0);     link.up();      return app.exec(); }

код функции main uav

int main(int argc, char* argv[]) {     QCoreApplication app(argc, argv);      // Для автопилота по-умолчанию sysid=1     domain::MavLinkCommunicator communicator(1, 0);      // Тип системы - самолёт с фиксированным крылом     domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_FIXED_WING);     QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived,                      &heartbeatHandler, &domain::HeartbeatHandler::processMessage);     // heartbeat отправляем на все доступные каналы связи      QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage,                      &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks);      // Настройки UDP через localhost     domain::UdpLink link(14551, QString("127.0.0.1"), 14550);     communicator.addLink(&link, MAVLINK_COMM_0);     link.up();      return app.exec(); }

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

Интересные ссылки:
Официальный сайт MAVLink
Сайт проекта Dronecode
Туториал на английском с сайта DIY Drones

Источник: habrahabr.ru

Разбираемся в MAVLink. Часть 1

Комментарии: