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

image

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 является пакет, имеющий следующий формат:

image

Первый байт пакета (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