October 11, 2016 at 15:16Understanding MAVLink. Part 1
To exchange data, many modern drones assembled by enthusiasts, commercial or even industrial, use the MAVLink protocol . I would like to share my experience with this protocol in this, and maybe in subsequent articles.
MAVLink or Micro Air Vehicle Link is a protocol of information interaction with drones or small unmanned aerial vehicles (flying, floating, crawling, etc.), hereinafter referred to as MAV (Micro Air Vehicle). MAVLink is distributed under the LGPL license in the form of a module for python (there is a convenient DroneKit wrapper ) and a library generator for various languages, including header-only C / C ++ libraries. There are also repositories of already generated libraries for MAVLink version v1 (this we will use) andversion v2 .
The protocol describes the information interaction between systems, such as MAV and GCS (Ground control station) - a ground control station, as well as their constituent parts - components. The basic essence of MAVLink is a package that has the following format:
The first byte of the packet ( STX ) is the message start character: 0xFD for version v2.0, 0xFE for version v1.0, 0x55 for version v0.9. LEN - payload length (messages). SEQ - contains the packet counter (0-255), which will help us identify the message loss. SYS (System ID) is the identifier of the sending system, and COMP (Component ID) is the identifier of the sending component. Msg(Message ID) - type of message, it depends on what data will be in the payload of the packet. PAYLOAD - packet payload, message, size from 0 to 255 bytes. The last two bytes of the packet - CKA and CKB , the lower and upper bytes, respectively, contain the checksum of the packet.
The MAVLink library allows you to encode and decode packets according to the protocol, but it does not regulate what hardware and software the data will be sent to - it can be TCP / UDP messages, serial communication, or anything that provides two-way communication. The library processes the input data by byte, adding it to the buffer and itself collects a packet from them. Each system or component can simultaneously exchange data on different sources, then for each source a special identifier is assigned, called channel . MAVLink contains a buffer on each channel.
Let's move from theory to practice and try to write an OOP wrapper on top of MAVLink. Below, I will give examples of C ++ code using Qt. I chose this tool, firstly, because in the future I plan to visualize some MAVLink parameters using Qt Quick and Qt Location, and secondly, I looked at many solutions in the qgroundcontrol project , also written in Qt.
To begin with, we introduce an abstraction over the communication channel, let it be the AbstractLink class, in its interface we define the functionality that allows us to receive and send data in the form of QByteArray. The heirs of this class, UdpLink and SerialLink, will provide us with data transmission over the network and through the serial port.
Encapsulate direct work with the MAVLink protocol in the MavLinkCommunicator class. His duties will include receiving data on communication channels and decoding them into mavlink_message_t packets, as well as sending messages on communication channels. Since, for each communication channel, MAVLink contains its own buffer, we will introduce a dictionary of the pointer to the communication channel to the channel identifier.
Consider how the package is assembled from the data stream. As mentioned above, MAVLink reads the incoming data stream by byte, for this, the mavlink_parse_char function is used, which returns message data or NULL if the message cannot be received, saving the received character to the internal buffer. MAVLink contains a buffer for each channel. This approach allows you to transfer data from the serial port directly to the MAVLink parsing function, depriving the user of the pleasure library to manually collect messages from the stream.
To obtain useful data, package assembly alone is not enough. It is necessary to receive a message from the package , extract the payload according to the msgid identifier. MAVLink has a set of built-in types for each msgid (message type) and functions for receiving these messages from the package. We introduce another abstract type - AbstractHandler, in the interface of this class we define a pure virtual slot processMessage for processing the message received from MavLinkCommunicator. The descendants of the AbstractHandler class will decide whether they can process a particular message and, if possible, process it. For example, we want to process messages like heartbeat. This is the most basic package in which the system says that it exists and what it is. It is worth noting that heartbeat is the only type of package that MAVLink requires to use. Let's introduce a message handler of this type - HeartbeatHandler.
Now, if we configure the classes and establish the correct connection, we can receive heartbeat messages from the flight controller. I will use a couple of radio modems and a Raspberry Pi with the NAVIO2 shield on which the APM autopilot is running. Theoretically, this should work with any autopilot that supports the current version of MAVLink, but if you don’t have anything at hand, a little further will be an example with an autopilot simulator.
We start the program, turn on the autopilot and after a few seconds should run:
As planned, each system should send a heartbeat, and therefore ours too. Let's start by implementing the function of sending a package of the MavLinkCommunicator class. The mavlink_msg_to_send_buffer function writes a message packet to the buffer for sending. It is assumed that at this stage all the fields of the packet, including the length and checksum, are filled in correctly.
Now that we have the function of sending a packet, we only have to compose a message and write it to the packet. We entrust this task to the existing HeartbeatHandler class, and add a message sending signal to the AbstractHandler interface. The mavlink_msg_heartbeat_encode function writes a heartbeat message to a packet, similar functions exist for all built-in messages. I’ll draw your attention to the fact that mavlink also provides additional functions, for example, mavlink_msg_heartbeat_pack allows you to write a heartbeat message to mavlink_message_t without explicitly creating an object of type mavlink_heartbeat_t, and mavlink_msg_heartbeat_send immediately sends data if there is a certain send function. More details on how to work with these functions can be found here.. The optional _chan ending (for example mavlink_msg_heartbeat_pack_chan) indicates which channel the message will be sent to.
We will send a heartbeat on a timer with a frequency of 1 Hz. If we put the debug output in the method of sending data of the communication channel data.toHex (), we will see our messages, according to the picture at the beginning of the article. Each clock counter should increase, and the checksum should change accordingly.
In order to check whether our heartbeat is working, we will create two assembly targets: gcs - a simulator of a ground control station and uav - a simulator of a drone.
The result should be a two-way exchange of heartbeat packets. If you wish, you can experiment further: add another system or communication channel. The full source code for this example can be found on the github . I hope it was interesting, even though the first part came out quite simple. In the next article I will try to talk about other types of messages and what interesting things can be done with them. Thank you for attention!
Interesting links:
Official MAVLink website Dronecode
project website
Tutorial in English from DIY Drones website
MAVLink or Micro Air Vehicle Link is a protocol of information interaction with drones or small unmanned aerial vehicles (flying, floating, crawling, etc.), hereinafter referred to as MAV (Micro Air Vehicle). MAVLink is distributed under the LGPL license in the form of a module for python (there is a convenient DroneKit wrapper ) and a library generator for various languages, including header-only C / C ++ libraries. There are also repositories of already generated libraries for MAVLink version v1 (this we will use) andversion v2 .
The protocol describes the information interaction between systems, such as MAV and GCS (Ground control station) - a ground control station, as well as their constituent parts - components. The basic essence of MAVLink is a package that has the following format:
The first byte of the packet ( STX ) is the message start character: 0xFD for version v2.0, 0xFE for version v1.0, 0x55 for version v0.9. LEN - payload length (messages). SEQ - contains the packet counter (0-255), which will help us identify the message loss. SYS (System ID) is the identifier of the sending system, and COMP (Component ID) is the identifier of the sending component. Msg(Message ID) - type of message, it depends on what data will be in the payload of the packet. PAYLOAD - packet payload, message, size from 0 to 255 bytes. The last two bytes of the packet - CKA and CKB , the lower and upper bytes, respectively, contain the checksum of the packet.
The MAVLink library allows you to encode and decode packets according to the protocol, but it does not regulate what hardware and software the data will be sent to - it can be TCP / UDP messages, serial communication, or anything that provides two-way communication. The library processes the input data by byte, adding it to the buffer and itself collects a packet from them. Each system or component can simultaneously exchange data on different sources, then for each source a special identifier is assigned, called channel . MAVLink contains a buffer on each channel.
We get heartbeat from the board
Let's move from theory to practice and try to write an OOP wrapper on top of MAVLink. Below, I will give examples of C ++ code using Qt. I chose this tool, firstly, because in the future I plan to visualize some MAVLink parameters using Qt Quick and Qt Location, and secondly, I looked at many solutions in the qgroundcontrol project , also written in Qt.
To begin with, we introduce an abstraction over the communication channel, let it be the AbstractLink class, in its interface we define the functionality that allows us to receive and send data in the form of QByteArray. The heirs of this class, UdpLink and SerialLink, will provide us with data transmission over the network and through the serial port.
Class interface 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);
};
Encapsulate direct work with the MAVLink protocol in the MavLinkCommunicator class. His duties will include receiving data on communication channels and decoding them into mavlink_message_t packets, as well as sending messages on communication channels. Since, for each communication channel, MAVLink contains its own buffer, we will introduce a dictionary of the pointer to the communication channel to the channel identifier.
MavLinkCommunicator class interface
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 m_linkChannels;
AbstractLink* m_lastReceivedLink;
};
Consider how the package is assembled from the data stream. As mentioned above, MAVLink reads the incoming data stream by byte, for this, the mavlink_parse_char function is used, which returns message data or NULL if the message cannot be received, saving the received character to the internal buffer. MAVLink contains a buffer for each channel. This approach allows you to transfer data from the serial port directly to the MAVLink parsing function, depriving the user of the pleasure library to manually collect messages from the stream.
MavLinkCommunicator class package build method
void MavLinkCommunicator::onDataReceived(const QByteArray& data)
{
mavlink_message_t message;
mavlink_status_t status;
// onDataReceived это слот, который вызываться строго по сигналу от AbstractLink
m_lastReceivedLink = qobject_cast(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); // по этому сигналу происходит обработка принятого пакета
}
// Обработка статуса канала связи
}
To obtain useful data, package assembly alone is not enough. It is necessary to receive a message from the package , extract the payload according to the msgid identifier. MAVLink has a set of built-in types for each msgid (message type) and functions for receiving these messages from the package. We introduce another abstract type - AbstractHandler, in the interface of this class we define a pure virtual slot processMessage for processing the message received from MavLinkCommunicator. The descendants of the AbstractHandler class will decide whether they can process a particular message and, if possible, process it. For example, we want to process messages like heartbeat. This is the most basic package in which the system says that it exists and what it is. It is worth noting that heartbeat is the only type of package that MAVLink requires to use. Let's introduce a message handler of this type - HeartbeatHandler.
Implementing the processMessage method of the HeartbeatHandler class
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;
}
Now, if we configure the classes and establish the correct connection, we can receive heartbeat messages from the flight controller. I will use a couple of radio modems and a Raspberry Pi with the NAVIO2 shield on which the APM autopilot is running. Theoretically, this should work with any autopilot that supports the current version of MAVLink, but if you don’t have anything at hand, a little further will be an example with an autopilot simulator.
function code 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();
}
We start the program, turn on the autopilot and after a few seconds should run:
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: 5Send your heartbeat
As planned, each system should send a heartbeat, and therefore ours too. Let's start by implementing the function of sending a package of the MavLinkCommunicator class. The mavlink_msg_to_send_buffer function writes a message packet to the buffer for sending. It is assumed that at this stage all the fields of the packet, including the length and checksum, are filled in correctly.
Method for sending a package of the MavLinkCommunicator class
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));
}
Now that we have the function of sending a packet, we only have to compose a message and write it to the packet. We entrust this task to the existing HeartbeatHandler class, and add a message sending signal to the AbstractHandler interface. The mavlink_msg_heartbeat_encode function writes a heartbeat message to a packet, similar functions exist for all built-in messages. I’ll draw your attention to the fact that mavlink also provides additional functions, for example, mavlink_msg_heartbeat_pack allows you to write a heartbeat message to mavlink_message_t without explicitly creating an object of type mavlink_heartbeat_t, and mavlink_msg_heartbeat_send immediately sends data if there is a certain send function. More details on how to work with these functions can be found here.. The optional _chan ending (for example mavlink_msg_heartbeat_pack_chan) indicates which channel the message will be sent to.
HeartbeatHandler timerEvent event code
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);
}
We will send a heartbeat on a timer with a frequency of 1 Hz. If we put the debug output in the method of sending data of the communication channel data.toHex (), we will see our messages, according to the picture at the beginning of the article. Each clock counter should increase, and the checksum should change accordingly.
"fe09000100000821ee85017f0000023f08"
"fe09010100000821ee85017f000002d576"
"fe09020100000821ee85017f000002ebf5"In order to check whether our heartbeat is working, we will create two assembly targets: gcs - a simulator of a ground control station and uav - a simulator of a drone.
function code 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();
}
function code 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();
}
The result should be a two-way exchange of heartbeat packets. If you wish, you can experiment further: add another system or communication channel. The full source code for this example can be found on the github . I hope it was interesting, even though the first part came out quite simple. In the next article I will try to talk about other types of messages and what interesting things can be done with them. Thank you for attention!
Interesting links:
Official MAVLink website Dronecode
project website
Tutorial in English from DIY Drones website