Повідомлення MAVLink
MAVLink це дуже легкий протокол обміну повідомленнями, який був спроектований для екосистеми дронів.
PX4 використовує MAVLink для зв'язку з наземними станціями і MAVLink SDK, такими як QGroundControl і MAVSDK, а також як інтеграційний механізм для підключення до компонентів дрона за межами польотного контролера: комп'ютерів-компаньйонів, камер з підтримкою MAVLink і так далі.
Ця тема надає короткий огляд основних концепцій MAVLink, таких як повідомлення, команди та мікросервіси. Він також надає інструкції посібника про те, як ви можете додати підтримку PX4 для:
- Потокових повідомлень MAVLink
- Обробка вхідних повідомлень MAVLink та запис до теми uORB.
Ця тема не охоплює обробку та надсилання команд або реалізацію власних мікросервісів.
Огляд MAVLink
MAVLink - це легкий протокол, який був розроблений для ефективної відправки повідомлень по ненадійним радіоканалах з низькою пропускною здатністю.
Повідомлення є найпростішим і найбільш "фундаментальним" визначенням у MAVLink, що складається з назви (наприклад, ATTITUDE), ідентифікатора та полів, що містять відповідні дані. Вони навмисно легкі, мають обмежений розмір і не мають семантики для повторного надсилання та підтвердження. Окремі повідомлення зазвичай використовуються для потокової передачі телеметрії або інформації про стан, а також для надсилання команд, які не потребують підтвердження - наприклад, команд уставки, що надсилаються з високою швидкістю.
Command Protocol - це протокол вищого рівня для надсилання команд, які можуть потребувати підтвердження. Конкретні команди визначаються як значення списку MAV_CMD, наприклад, команда зльоту MAV_CMD_NAV_TAKEOFF, і включають до 7 числових значень "param". Протокол надсилає команду, упаковуючи значення параметрів у повідомлення COMMAND_INT
або COMMAND_LONG
, і чекає на підтвердження з результатом у COMMAND_ACK
. Якщо команда не буде отримана, вона буде повторно надіслана автоматично. Зауважте, що визначення MAV_CMD також використовуються для визначення дій місії, і що не всі визначення підтримуються для використання у командах/місіях на PX4.
Мікросервіси - це інші протоколи вищого рівня, побудовані на основі повідомлень MAVLink. Вони використовуються для передачі інформації, яку неможливо надіслати одним повідомленням, а також для забезпечення таких функцій, як надійний зв'язок. Описаний вище командний протокол є одним з таких сервісів. Інші включають Протокол передачі файлів, Протокол камери і Протокол місії.
MAVLink повідомлення, команди та переліки визначаються у XML-файлах визначень. Інструментарій MAVLink включає в себе генератори коду, які створюють з цих визначень специфічні для мови програмування бібліотеки для надсилання та отримання повідомлень. Зверніть увагу, що більшість згенерованих бібліотек не створюють код для реалізації мікросервісів.
Проект MAVLink стандартизує ряд повідомлень, команд, переліків та мікросервісів для обміну даними за допомогою наступних файлів визначень (зауважте, що файли вищого рівня _ включають_ визначення файлів нижчого рівня):
- development.xml - Визначення, які пропонується включити до стандарту. Визначення переміщуються до
common.xml
, якщо їх прийнято після тестування. - common.xml - "бібліотека" визначень, що відповідають багатьом поширеним випадкам використання БПЛА. Вони підтримуються багатьма польотними стеками, наземними станціями та периферійними пристроями MAVLink. Польотні стеки, які використовують ці визначення, з більшою ймовірністю будуть взаємодіяти.
- standard.xml - Визначення, які є стандартними. Вони присутні на переважній більшості польотних стеків і реалізовані однаково.
- minimal.xml - Визначення, необхідні для мінімальної реалізації MAVLink.
Проект також містить діалектні XML-визначення, які містять визначення MAVLink, специфічні для польотного стеку або інших зацікавлених сторін.
Протокол покладається на те, що кожна сторона комунікації має спільне визначення того, які повідомлення надсилаються. Це означає, що для того, щоб взаємодіяти, обидва кінці комунікації повинні використовувати бібліотеки, створені на основі одного і того ж визначення XML.
PX4 та MAVLink
PX4 за замовчуванням випускає збірку common.xml
визначень MAVLink для забезпечення максимальної сумісності з наземними станціями MAVLink, бібліотеками та зовнішніми компонентами, такими як камери MAVLink. У гілці main
вони містяться у development.xml
на SITL та common.xml
для інших плат.
Щоб бути частиною випуску PX4, всі визначення MAVLink, які ви використовуєте, повинні знаходитися у common.xml
(або у включених файлах, таких як standard.xml
та minimal.xml
). Під час розробки ви можете використовувати визначення в development.xml
. Вам потрібно буде попрацювати з командою MAVLink, щоб визначити і внести ці визначення.
PX4 включає репозиторій mavlink/mavlink як підмодуль у /src/modules/mavlink. Тут містяться файли визначень XML у каталозі /mavlink/messages/1.0/.
Інструментарій збірки генерує заголовні файли MAVLink 2 C під час збірки. XML-файл, для якого генеруються файли заголовків, можна визначити у конфігурації плати PX4 kconfig для кожної окремої плати за допомогою змінної CONFIG_MAVLINK_DIALECT
:
- Для SITL
CONFIG_MAVLINK_DIALECT
встановлено уdevelopment
у boards/px4/sitl/default.px4board. Ви можете змінити його на будь-який інший файл визначення, але він повинен міститиcommon.xml
. - Для інших плат
CONFIG_MAVLINK_DIALECT
не встановлено за замовчуванням, і PX4 збирає визначення уcommon.xml
(за замовчуванням вони вбудовані у mavlink module - шукайтеmenuconfig MAVLINK_DIALECT
у src/modules/mavlink/Kconfig).
Файли генеруються до каталогу збірки: /build/<build target>/mavlink/
.
Custom MAVLink Messages
Користувацьке повідомлення MAVLink - це повідомлення, якого немає у визначеннях за замовчуванням, включених до PX4.
INFO
Якщо ви використовуєте користувацьке визначення, вам потрібно буде підтримувати визначення у PX4, вашій наземній станції та будь-яких інших SDK, які взаємодіють з нею. Загалом, щоб зменшити тягар обслуговування, слід використовувати (або доповнювати) стандартні визначення, якщо це можливо.
Користувацькі визначення можна додати до нового файлу діалекту у тому самому каталозі, що й стандартні визначення XML. Наприклад, створіть PX4-Autopilot/src/modules/mavlink/mavlink/mavlink/message_definitions/v1.0/custom_messages.xml
і встановіть CONFIG_MAVLINK_DIALECT
для створення нового файла для SITL. Цей файл діалекту має містити development.xml
, щоб до нього було включено всі стандартні визначення.
Для початкового прототипування, або якщо ви плануєте, що ваше повідомлення буде "стандартним", ви також можете додати свої повідомлення до common.xml
(або development.xml
). Це спрощує збірку, оскільки вам не потрібно модифікувати вже зібраний діалект.
Посібник розробника MAVLink пояснює, як визначити нові повідомлення у розділі How to Define MAVLink Messages & Enums.
Ви можете перевірити, що ваші нові повідомлення зібрано, переглянувши заголовки, згенеровані у каталозі збірки (/build/<build target>/mavlink/
). Якщо ваші повідомлення не збираються, вони можуть бути неправильно відформатовані або використовувати конфліктуючі ідентифікатори. Перевірте журнал збірки для отримання інформації.
Після того, як повідомлення створено, ви можете передавати, отримувати або використовувати його в інший спосіб, як описано в наступних розділах.
Посібник MAVLink Developer Guide містить більше інформації про використання інструментарію MAVLink. :::
Потокові повідомлення MAVLink
Повідомлення MAVLink транслюються за допомогою потокового класу, похідного від MavlinkStream
, який було додано до списку потоків PX4. Клас має фреймворкові методи, які ви реалізуєте, щоб PX4 міг отримати потрібну йому інформацію зі згенерованого визначення повідомлення MAVLink. Він також має метод send()
, який викликається кожного разу, коли потрібно надіслати повідомлення - ви перевизначаєте його, щоб скопіювати інформацію з підписки uORB в об'єкт повідомлення MAVLink, який потрібно надіслати.
Цей посібник демонструє, як транслювати повідомлення uORB як повідомлення MAVLink, і застосовується як до стандартних, так і до користувацьких повідомлень.
Передумови
Загалом у вас вже повинно бути повідомлення uORB, яке містить інформацію, яку ви хочете транслювати, та визначення повідомлення MAVLink, з яким ви хочете його транслювати.
For this example we're going to assume that you want to stream the (existing) BatteryStatus uORB message to a new MAVLink battery status message, which we will name BATTERY_STATUS_DEMO
.
Скопіюйте це повідомлення BATTERY_STATUS_DEMO
у розділ повідомлень development.xml
у вихідному коді PX4, який буде розташований за адресою: \src\modules\mavlink\mavlink\message_definitions\v1.0\development.xml
.
xml
<message id="11514" name="BATTERY_STATUS_DEMO">
<description>Simple demo battery.</description>
<field type="uint8_t" name="id" instance="true">Battery ID</field>
<field type="int16_t" name="temperature" units="cdegC" invalid="INT16_MAX">Temperature of the whole battery pack (not internal electronics). INT16_MAX field not provided.</field>
<field type="uint8_t" name="percent_remaining" units="%" invalid="UINT8_MAX">Remaining battery energy. Values: [0-100], UINT8_MAX: field not provided.</field>
</message>
Note that this is a cut-down version of the not-yet-implemented BATTERY_STATUS_V2 message with randomly chosen unused id of 11514
. Here we've put the message in development.xml
, which is fine for testing and if the message is intended to eventually be part of the standard message set, but you might also put a custom message in its own dialect file.
Build PX4 for SITL and confirm that the associated message is generated in /build/px4_sitl_default/mavlink/common/mavlink_msg_battery_status_demo.h
.
Because BatteryStatus
already exists you will not need to do anything to create or build it.
Define the Streaming Class
First create a file named BATTERY_STATUS_DEMO.hpp
for your streaming class (named after the message to stream) inside the /src/modules/mavlink/streams directory.
Add the headers for the uORB message(s) to the top of the file (the required MAVLink headers should already be available):
cpp
#include <uORB/topics/battery_status.h>
INFO
The uORB topic's snake-case header file is generated from the CamelCase uORB filename at build time.
Then copy the streaming class definition below into the file:
cpp
class MavlinkStreamBatteryStatusDemo : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamBatteryStatusDemo(mavlink);
}
const char *get_name() const
{
return MavlinkStreamBatteryStatusDemo::get_name_static();
}
static const char *get_name_static()
{
return "BATTERY_STATUS_DEMO";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_BATTERY_STATUS_DEMO;
}
uint16_t get_id()
{
return get_id_static();
}
unsigned get_size()
{
return MAVLINK_MSG_ID_BATTERY_STATUS_DEMO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
//Subscription to array of uORB battery status instances
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
// SubscriptionMultiArray subscription is needed because battery has multiple instances.
// uORB::Subscription is used to subscribe to a single-instance topic
/* do not allow top copying this class */
MavlinkStreamBatteryStatusDemo(MavlinkStreamBatteryStatusDemo &);
MavlinkStreamBatteryStatusDemo& operator = (const MavlinkStreamBatteryStatusDemo &);
protected:
explicit MavlinkStreamBatteryStatusDemo(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
bool updated = false;
// Loop through _battery_status_subs (subscription to array of BatteryStatus instances)
for (auto &battery_sub : _battery_status_subs) {
// battery_status_s is a struct that can hold the battery object topic
battery_status_s battery_status;
// Update battery_status and publish only if the status has changed
if (battery_sub.update(&battery_status)) {
// mavlink_battery_status_demo_t is the MAVLink message object
mavlink_battery_status_demo_t bat_msg{};
bat_msg.id = battery_status.id - 1;
bat_msg.battery_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : -1;
// check if temperature valid
if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) {
bat_msg.temperature = battery_status.temperature * 100.f;
} else {
bat_msg.temperature = INT16_MAX;
}
//Send the message
mavlink_msg_battery_status_demo_send_struct(_mavlink->get_channel(), &bat_msg);
updated = true;
}
}
return updated;
}
};
Most streaming classes are very similar (see examples in /src/modules/mavlink/streams):
The streaming class derives from
MavlinkStream
and is named using the patternMavlinkStream<CamelCaseMessageName>
.The
public
definitions are "near-boilerplate", allowing PX4 to get an instance of the class (new_instance()
), and then to use it to fetch the name, id, and size of the message from the MAVLink headers (get_name()
,get_name_static()
,get_id_static()
,get_id()
,get_size()
). For your own streaming classes these can just be copied and modified to match the values for your MAVLink message.The
private
definitions subscribe to the uORB topics that need to be published. In this case the uORB topic has multiple instances: one for each battery. We useuORB::SubscriptionMultiArray
to get an array of battery status subscriptions.Here we also define constructors to prevent the definition being copied.
The
protected
section is where the important work takes place!Here we override the
send()
method, copying values from the subscribed uORB topic(s) into appropriate fields in the MAVLink message, and then send the message.In this particular example we have an array of uORB instances
_battery_status_subs
(because we have multiple batteries). We iterate the array and useupdate()
on each subscription to check if the associated battery instance has changed (and update a structure with the current data). This allows us to send the MAVLink message only if the associated battery uORB topic has changed:cpp// Структура, щоб зберігати дані поточної теми. battery_status_s battery_status; // update() populates battery_status and returns true if the status has changed if (battery_sub.update(&battery_status)) { // Use battery_status to populate message and send }
If wanted to send a MAVLink message whether or not the data changed, we could instead use
copy()
as shown:cppbattery_status_s battery_status; battery_sub.copy(&battery_status);
For a single-instance topic like VehicleStatus we would subscribe like this:
cpp// Create subscription _vehicle_status_sub uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
And we could use the resulting subscription in the same way with update or copy.
cppvehicle_status_s vehicle_status{}; // vehicle_status_s is the definition of the uORB topic if (_vehicle_status_sub.update(&vehicle_status)) { // Use the vehicle_status as it has been updated. }
:::
Next we include our new class in mavlink_messages.cpp. Add the line below to the part of the file where all the other streams are included:
cpp
#include "streams/BATTERY_STATUS_DEMO.hpp"
Finally append the stream class to the streams_list
at the bottom of mavlink_messages.cpp
C
StreamListItem *streams_list[] = {
...
#if defined(BATTERY_STATUS_DEMO_HPP)
create_stream_list_item<MavlinkStreamBatteryStatusDemo>(),
#endif // BATTERY_STATUS_DEMO_HPP
...
}
The class is now available for streaming, but won't be streamed by default. We cover that in the next sections.
Streaming by Default
The easiest way to stream your messages by default (as part of a build) is to add them to mavlink_main.cpp in the appropriate message group.
If you search in the file you'll find groups of messages defined in a switch statement:
MAVLINK_MODE_NORMAL
: Streamed to a GCS.MAVLINK_MODE_ONBOARD
: Streamed to a companion computer on a fast link, such as EthernetMAVLINK_MODE_ONBOARD_LOW_BANDWIDTH
: Streamed to a companion computer for re-routing to a reduced-traffic link, such as a GCS.MAVLINK_MODE_GIMBAL
: Streamed to a gimbalMAVLINK_MODE_EXTVISION
: Streamed to an external vision systemMAVLINK_MODE_EXTVISIONMIN
: Streamed to an external vision system on a slower linkMAVLINK_MODE_OSD
: Streamed to an OSD, such as an FPV headset.MAVLINK_MODE_CUSTOM
: Stream nothing by default. Used when configuring streaming using MAVLink.MAVLINK_MODE_MAGIC
: Same asMAVLINK_MODE_CUSTOM
MAVLINK_MODE_CONFIG
: Streaming over USB with higher rates thanMAVLINK_MODE_NORMAL
.MAVLINK_MODE_MINIMAL
: Stream a minimal set of messages. Normally used for poor telemetry links.MAVLINK_MODE_IRIDIUM
: Streamed to an iridium satellite phone
Normally you'll be testing on a GCS, so you could just add the message to the MAVLINK_MODE_NORMAL
case using the configure_stream_local()
method. For example, to stream CA_TRAJECTORY at 5 Hz:
cpp
case MAVLINK_MODE_CONFIG: // USB
// Note: streams requiring low latency come first
...
configure_stream_local("BATTERY_STATUS_DEMO", 5.0f);
...
It is also possible to add a stream by calling the mavlink module with the stream
argument in a startup script. For example, you might add the following line to /ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink in order to stream BATTERY_STATUS_DEMO
at 50Hz on UDP port 14556
(-r
configures the streaming rate and -u
identifies the MAVLink channel on UDP port 14556).
sh
mavlink stream -r 50 -s BATTERY_STATUS_DEMO -u 14556
Streaming on Request
Some messages are only needed once, when particular hardware is connected, or under other circumstances. In order to avoid clogging communications links with messages that aren't needed you may not stream all messages by default, even at low rate.
If you needed, a GCS or other MAVLink API can request that particular messages are streamed at a particular rate using MAV_CMD_SET_MESSAGE_INTERVAL. A particular message can be requested just once using MAV_CMD_REQUEST_MESSAGE.
Receiving MAVLink Messages
This section explains how to receive a message over MAVLink and publish it to uORB.
It assumes that we are receiving the BATTERY_STATUS_DEMO
message and we want to update the (existing) BatteryStatus uORB message with the contained information. This is the kind of implementation that you would provide to support a MAVLink battery integration with PX4.
Add the headers for the uORB topic to publish to in mavlink_receiver.h:
cpp
#include <uORB/topics/battery_status.h>
Add a function signature for a function that handles the incoming MAVLink message in the MavlinkReceiver
class in mavlink_receiver.h
cpp
void handle_message_battery_status_demo(mavlink_message_t *msg);
Normally you would add a uORB publisher for the uORB topic to publish in the MavlinkReceiver
class in mavlink_receiver.h. In this case the BatteryStatus uORB topic already exists:
cpp
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
This creates a publication to a single uORB topic instance, which by default will be the first instance.
This implementation won't work on multi-battery systems, because several batteries might be publishing data to the first instance of the topic, and there is no way to differentiate them. To support multiple batteries we'd need to use PublicationMulti
and map the MAVLink message instance IDs to specific uORB topic instances.
Implement the handle_message_battery_status_demo
function in mavlink_receiver.cpp.
cpp
void
MavlinkReceiver::handle_message_battery_status_demo(mavlink_message_t *msg)
{
if ((msg->sysid != mavlink_system.sysid) || (msg->compid == mavlink_system.compid)) {
// ignore battery status coming from other systems or from the autopilot itself
return;
}
// external battery measurements
mavlink_battery_status_t battery_mavlink;
mavlink_msg_battery_status_decode(msg, &battery_mavlink);
battery_status_s battery_status{};
battery_status.timestamp = hrt_absolute_time();
battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
battery_status.temperature = (float)battery_mavlink.temperature;
battery_status.connected = true;
_battery_pub.publish(battery_status);
}
INFO
Above we only write to the battery fields that are defined in the topic. In practice you'd update all fields with either valid or invalid values: this has been cut back for brevity.
and finally make sure it is called in MavlinkReceiver::handle_message()
cpp
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
switch (msg->msgid) {
...
case MAVLINK_MSG_ID_BATTERY_STATUS_DEMO:
handle_message_battery_status_demo(msg);
break;
...
}
}
Alternative to Creating Custom MAVLink Messages
Sometimes there is the need for a custom MAVLink message with content that is not fully defined.
For example when using MAVLink to interface PX4 with an embedded device, the messages that are exchanged between the autopilot and the device may go through several iterations before they are stabilized. In this case, it can be time-consuming and error-prone to regenerate the MAVLink headers, and make sure both devices use the same version of the protocol.
An alternative - and temporary - solution is to re-purpose debug messages. Instead of creating a custom MAVLink message CA_TRAJECTORY
, you can send a message DEBUG_VECT
with the string key CA_TRAJ
and data in the x
, y
and z
fields. See this tutorial for an example usage of debug messages.
INFO
This solution is not efficient as it sends character string over the network and involves comparison of strings. It should be used for development only!
Testing
As a first step, and while debugging, commonly you'll just want to confirm that any messages you've created are being sent/received as you expect.
You should should first use the uorb top [<message_name>]
command to verify in real-time that your message is published and the rate (see uORB Messaging). This approach can also be used to test incoming messages that publish a uORB topic (for other messages you might use printf
in your code and test in SITL).
There are several approaches you can use to view MAVLink traffic:
Create a Wireshark MAVLink plugin for your dialect. This allows you to inspect MAVLink traffic on an IP interface - for example between QGroundControl or MAVSDK and your real or simulated version of PX4.
TIP
It is much easier to generate a wireshark plugin and inspect traffic in Wireshark, than to rebuild QGroundControl with your dialect and use MAVLink Inspector. :::
- Log uORB topics associate with your MAVLink message.
- View received messages in the QGroundControl MAVLink Inspector. You will need to rebuild QGroundControl with the custom message definitions, as described below
Set Streaming Rate using a Shell
For testing, it is sometimes useful to increase the streaming rate of individual topics at runtime (e.g. for inspection in QGC). This can be achieved using by calling the mavlink module through the QGC MAVLink console (or some other shell):
sh
mavlink stream -u <port number> -s <mavlink topic name> -r <rate>
You can get the port number with mavlink status
which will output (amongst others) transport protocol: UDP (<port number>)
. An example would be:
sh
mavlink stream -u 14556 -s CA_TRAJECTORY -r 300
Updating Ground Stations
Ultimately you'll want to use your new MAVLink interface by providing the corresponding ground station or MAVSDK implementation.
The important thing to remember here is that MAVLink requires that you use a version of the library that is built to the same definition (XML file). So if you have created a custom message in PX4 you won't be able to use it unless you build QGC or MAVSDK with that same definition.
Updating QGroundControl
You will need to Build QGroundControl including a pre-built C library that contains your custom messages.
QGC uses a pre-built C library that must be located at /qgroundcontrol/libs/mavlink/include/mavlink in the QGC source.
By default this is pre-included as a submodule from https://github.com/mavlink/c_library_v2 but you can generate your own MAVLink Libraries.
QGC uses the all.xml dialect by default, which includes common.xml. You can include your messages in either file or in your own dialect. However if you use your own dialect then it should include ArduPilotMega.xml (or it will miss all the existing messages), and you will need to change the dialect used by setting it in MAVLINK_CONF
when running qmake.
Оновлення MAVSDK
See the MAVSDK docs for information about how to work with MAVLink headers and dialects.