Розбираємося в MAVLink. Частина 1

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

image

MAVLink або Micro Air Vehicle Link — це протокол інформаційного взаємодії з дрона або малими безпілотними апаратами (літаючими, плаваючими, повзаючими тощо), далі за текстом званих MAV (Micro Air Vehicle). MAVLink поширюється під ліцензією LGPL у вигляді модуля для python (є зручна обгортка DroneKit) і генератора header-only С/C++ бібліотеки. Є так само репозиторії вже згенерованих бібліотек для MAVLink <a href=«github.com/mavlink/c_library_v1»версії>v1 (цього ми і будемо користуватися) і <a href=«github.com/mavlink/c_library_v2»версії>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) — ідентифікатор відправляє системи, а COM (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);

віртуальний 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. При бажанні можна експериментувати далі: додати ще одну систему або канал зв'язку. Повний вихідний код цього прикладу можна знайти на гітхабі. Сподіваюся, було цікаво, хоч перша частина вийшла досить простий. У наступній статті я постараюся розповісти про інші типи повідомлень і що цікавого з ними можна робити. Дякую за увагу!
Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.