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

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

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

11.10.2016 12:16

mrogachev 13 3800 Источник

Для обмена данными многие современные дроны, собираемые энтузиастами, коммерческие или даже промышленные, используют протокол 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

Поделиться с друзьями

-->

Комментарии (13)

Vladimir_Sklyar
11.10.2016 16:54
#9852540
Добрый день, а на практике какой-то беспилотник использовался?
По тексту: поле Component ID на рисунка названо COMP, а в тексте COM, может быть на рисунке лучше использовать поля, как названо в исходнике?
1. mrogachev
  11.10.2016 17:37
  #9852690
  +1
  Спасибо, поправил. На практике собираю самосборку в корпусе Skywalker X8 с Raspbery Pi3 * NAVIO2.
  
  Видео полёта в ручном режиме

NLO
11.10.2016 17:37
#9852692
НЛО прилетело и опубликовало эту надпись здесь
1. mrogachev
  11.10.2016 17:39
  #9852694
  По ссылке главная страница, на сайте MAVLink нет биндинга к заголовкам. Но я добавил ссылки в конце статьи
  1. NLO
    11.10.2016 17:46
    #9852718
    НЛО прилетело и опубликовало эту надпись здесь

Atilla
11.10.2016 19:41
#9852992
Из предложений: возможно имеет смысл написать за что отвечает System ID и Component ID.
Также имеет смысл сразу сделать вместо класса AbstractHandler/HeartbeatHandler абстрактный класс для каждого MAV, который бы отвечал за обработку принятых сообщений и упаковку сообщений для каждого MAV отдельно:
```
class AbstractMav: public QObject
{
    Q_OBJECT
    
public:
    AbstractMav(QObject* parent = nullptr, AbstractLink* link);

public slots:
    void sendHeartbeat(unit8_t compid);
    void arm();
    void disarm();
    ...

signals:
    void messageReady(const mavlink_message_t& message, const AbstractLink* link);
    void heartheatReceived(unit8_t compid);
    void imuReceived(unit8_t compid, uint64_t timestamp,
                     const QVector3D& accel, const QVector3D& gyro, const QVector3D& mag,
                     double pressure, double temperature);
    ...

private slots:
    void processMessage(const mavlink_message_t& message);

private:
    void commandLong(uint8_t compid,
                     uint16_t command,
                     uint8_t confirmation,
                     float param1, float param2, float param3, float param4,
                     float param5, float param6, float param7 );
    ...

    uint8_t m_sysid;
    AbstractLink* m_link;
};
```
слот AbstractMav::processMessage соединятеся с сигналом MavLinkCommunicator::messageReceived, и отфильтровыват сообщенния принадлежащие только ему:
```
void AbstractMav::processMessage(const mavlink_message_t& message) {
    if (message.sysid != m_sysid) {
        return;
    }
    
    switch(message.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT: {
            emit heartheatReceived(message.compid);
            break;
        }
        case MAVLINK_MSG_ID_HIGHRES_IMU: {
            ...
        }
    }
}
```
а сигнал AbstractMav::messageReady со слотом MavLinkCommunicator::sendMessage. Каждая функция, которая отправляет данные на MAV, должна активировать сигнал messageReady:
```
void AbstractMav::sendHeartbeat(unit8_t compid) {
    mavlink_message_t message;
    mavlink_heartbeat_t heartbeat;
    
    mavlink_msg_heartbeat_encode(m_sysd, compid, &message, &heartbeat);

    emit messageReady(message, m_link)
}
```
Правильная реализация MAVLink должна поддерживать общение с несколькими MAV через один канал, или даже через несколько каналов. Такую возможность лучше заложить сразу, а не переписывать потом.

И из мелочей: имеет смысл использовать QHash вместо QMap для члена MavLinkCommunicator:: m_linkChannels, так как упорядоченности по ключу нам не важна.
1. mikkab
  12.10.2016 08:31
  #9853936
  согласен по поводу работы с несколькими девайсами, но для подавляющего большинства это не самый важный момент, и все оставляют на потом.
  
  if (message.sysid != m_sysid) { return; }
  
  тут не совсем корректно, тк общие сообщения будут игнорироваться.
1. mrogachev
  12.10.2016 10:23
  #9854232
  Спасибо за советы, в следующей статье постараюсь учесть замечания.
  В конечной архитектуре у меня есть класс Vehicle, который является моделью дрона, но наполнение его происходит всё равно через наследников AbstractHandler. Это упрощённый вариант паттерна цепочка обязанностей, реализованный через механизм сигналов и слотов Qt. Его я ввёл, чтобы избавиться от switch(message.msgid). Heartbeat тогда бы выглядел так:
  
  void HeartbeatHandler::processMessage(const mavlink_message_t& message) { if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT) return; Vehicle* vehicle = m_vehicleService->requestVehicle(message.sysid); mavlink_heartbeat_t heartbeat; mavlink_msg_heartbeat_decode(&message, &heartbeat); vehicle->setType(::vehicleTypeFromMavLinkType(heartbeat.type)); vehicle->setState(::vehicleStateFromMavLinkSystemStatus(heartbeat.system_status)); // ... }
  
  К тому же, при такой реализации Vehicle не завязан на типы MAVLink, и в будущем можно добавить другой протокол. В следующей статье, как приведу код в порядок, нарисую диаграмму классов.
  
  QMap использовал т.к. на прогнозируемым количестве элементов он должен быть быстрее QHash. Основывался на этой статье.

mikkab
12.10.2016 08:32
#9853938
+2
ждем продолжения

Vooon
12.10.2016 13:23
#9854822
Сейчас уже лучше писать про версию 2.0, т.к. она совместима с 1.0 (выбирается через флаги).
2.0 уже поддерживается APM'ом и PX4. QGC пока в процессе перехода.

И еще, генератор поддерживает куда больше языков: Java, JavaScript, C#, Python, Object C, Swift.

А для C++ мне надоели неудобства C-lib и я написал C++11 генератор (вот только не знаю когда замержат).
1. mrogachev
  12.10.2016 14:11
  #9855004
  Спасибо за уточнения, если мой APM правда съест версию 2.0, вторую часть напишу на ней. Про генераторы подправил в статье. C++11 генератор интересно, можно посмотреть где-нибудь?)
  1. Vooon
    12.10.2016 16:08
    #9855376
    Не уверен, что APM 2.6 (что на atmega) уже поддерживает, проверь свежайшую прошивку.
    
    https://github.com/mavlink/mavlink/tree/mavlink2-cxx11 но его нужно теперь переносить в pymavlink и слить в 3-4 коммита.

Vooon
12.10.2016 16:07
#9855366
del