We understand in MAVLink. Part 1
For the exchange of data, many modern drones, collected 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 for information interaction with drones or small unmanned vehicles (flying, floating, crawling, etc.), hereinafter referred to as MAV (Micro Air Vehicle). MAVLink is distributed under the LGPL license as a module for python (there is a convenient wrapper for DroneKit ) and a library generator for various languages, including the header-only C / C ++ library. There are also repositories of already generated libraries for MAVLink version v1 (we will use this one) and version 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 character of the beginning of the message: 0xFD for version v2.0, 0xFE for version v1.0, 0x55 for version v0.9. LEN - the length of the payload (message). SEQ - contains a package counter (0-255), which will help us identify the loss of the message. SYS (System ID) is the sending system identifier, and COMP (Component ID) is the sending component identifier. MSG (Message ID) - type of message, it depends on what data will lie in the payload package. PAYLOAD - packet payload, message size from 0 to 255 bytes. The last two bytes of the packet - CKA and CKB , the lower and upper byte, 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 data will be sent - it can be TCP / UDP messages, exchange via a serial port, and anything that provides two-way exchange. The library processes the input data byte-by-byte, adding it to the buffer and assembles the packet from it itself. Each system or component can simultaneously exchange data from different sources, then a special identifier called a channel is assigned to each source. MAVLink contains a buffer for each channel.
Let's move from theory to practice and try to write an OOP wrapper over MAVLink. Below, I will provide code examples in C ++ using Qt. I chose this tool, firstly, because in the future I plan to visualize some of the MAVLink parameters using Qt Quick and Qt Location, and secondly, I spotted 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 via the serial port.
We work directly with the MAVLink protocol in the MavLinkCommunicator class. His responsibilities will include receiving data via communication channels and decoding them into mavlink_message_t packets, as well as sending messages via communication channels. Since, for each communication channel, MAVLink contains its buffer, we will introduce a dictionary of the pointer to the communication channel to the channel identifier.
Consider how to build the package from the data stream. As mentioned above, MAVLink reads the incoming data stream byte-by-by; the mavlink_parse_char function is used for this, which returns the message data or NULL if the message cannot be received, storing the received character in the internal buffer. MAVLink contains a buffer for each channel. This approach allows transferring data from the serial port directly to the parsing function of the MAVLink package, depriving the user of the library of the pleasure of manually collecting messages from the stream.
For obtaining the useful data of one only assembly of a packet is not enough. It is necessary to receive a message from the packet, extract the payload according to msgid. 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 purely virtual processMessage slot for processing the message received from MavLinkCommunicator. The heirs of the AbstractHandler class will decide whether they can process a message and, if possible, process it. For example, we want to process heartbeat messages. 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. We introduce a message handler of this type - HeartbeatHandler.
Now, if we set up the classes and establish the correct connection, we can receive heartbeat messages from the flight controller. I will use a pair 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 have nothing at hand, there will be an example with the autopilot simulator a little further.
Run the program, turn on the autopilot and after a few seconds should run:
As planned, each system must send a heartbeat, and therefore ours too. Let's start with the implementation of the function of sending the package class MavLinkCommunicator. The mavlink_msg_to_send_buffer function writes the message packet to the send buffer. It is assumed that at this stage all the packet fields, including the length and the checksum, are filled in correctly.
Now that we have the function to send a packet, we need to form a message and write it into the packet. Let us assign this task to the already existing HeartbeatHandler class, and add the message sending signal to the AbstractHandler interface. The mavlink_msg_heartbeat_encode function writes the heartbeat message to the package, there are similar functions for all embedded messages. I’ll draw the reader’s attention that additional functions are also provided in mavlink, 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 sending function. More information on how to work with these features can be found here . The additional ending _chan (for example mavlink_msg_heartbeat_pack_chan) indicates which channel the message will be sent to.
We will send heartbeat on a timer with a frequency of 1 Hz. If you put a debug output in the method of sending data to the communication channel data.toHex (), we will see our messages, according to the picture given at the beginning of the article. Each clock count should increase, and the checksum will change accordingly.
In order to check whether our heartbeat is working, we will create two assembly targets: gcs - ground control station simulator and uav - drone simulator.
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 of this example can be found on github . I hope it was interesting, even though the first part came out pretty simple. In the next article I will try to tell you about other types of messages and what interesting things you can do with them. Thank you for attention!
Interesting links:
MAVLink official website
Dronecode project site
Tutorial in English from the site DIY Drones
MAVLink or Micro Air Vehicle Link is a protocol for information interaction with drones or small unmanned vehicles (flying, floating, crawling, etc.), hereinafter referred to as MAV (Micro Air Vehicle). MAVLink is distributed under the LGPL license as a module for python (there is a convenient wrapper for DroneKit ) and a library generator for various languages, including the header-only C / C ++ library. There are also repositories of already generated libraries for MAVLink version v1 (we will use this one) and version 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 character of the beginning of the message: 0xFD for version v2.0, 0xFE for version v1.0, 0x55 for version v0.9. LEN - the length of the payload (message). SEQ - contains a package counter (0-255), which will help us identify the loss of the message. SYS (System ID) is the sending system identifier, and COMP (Component ID) is the sending component identifier. MSG (Message ID) - type of message, it depends on what data will lie in the payload package. PAYLOAD - packet payload, message size from 0 to 255 bytes. The last two bytes of the packet - CKA and CKB , the lower and upper byte, 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 data will be sent - it can be TCP / UDP messages, exchange via a serial port, and anything that provides two-way exchange. The library processes the input data byte-by-byte, adding it to the buffer and assembles the packet from it itself. Each system or component can simultaneously exchange data from different sources, then a special identifier called a channel is assigned to each source. MAVLink contains a buffer for each channel.
We get heartbeat from board
Let's move from theory to practice and try to write an OOP wrapper over MAVLink. Below, I will provide code examples in C ++ using Qt. I chose this tool, firstly, because in the future I plan to visualize some of the MAVLink parameters using Qt Quick and Qt Location, and secondly, I spotted 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 via the serial port.
AbstractLink class interface
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); }; We work directly with the MAVLink protocol in the MavLinkCommunicator class. His responsibilities will include receiving data via communication channels and decoding them into mavlink_message_t packets, as well as sending messages via communication channels. Since, for each communication channel, MAVLink contains its 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<AbstractLink*, uint8_t> m_linkChannels; AbstractLink* m_lastReceivedLink; }; Consider how to build the package from the data stream. As mentioned above, MAVLink reads the incoming data stream byte-by-by; the mavlink_parse_char function is used for this, which returns the message data or NULL if the message cannot be received, storing the received character in the internal buffer. MAVLink contains a buffer for each channel. This approach allows transferring data from the serial port directly to the parsing function of the MAVLink package, depriving the user of the library of the pleasure of manually collecting messages from the stream.
Build method of the MavLinkCommunicator class package
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); // } // } For obtaining the useful data of one only assembly of a packet is not enough. It is necessary to receive a message from the packet, extract the payload according to msgid. 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 purely virtual processMessage slot for processing the message received from MavLinkCommunicator. The heirs of the AbstractHandler class will decide whether they can process a message and, if possible, process it. For example, we want to process heartbeat messages. 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. We 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 set up the classes and establish the correct connection, we can receive heartbeat messages from the flight controller. I will use a pair 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 have nothing at hand, there will be an example with the autopilot simulator a little further.
main function code
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(); } Run 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: 5 Ship your heartbeat
As planned, each system must send a heartbeat, and therefore ours too. Let's start with the implementation of the function of sending the package class MavLinkCommunicator. The mavlink_msg_to_send_buffer function writes the message packet to the send buffer. It is assumed that at this stage all the packet fields, including the length and the checksum, are filled in correctly.
MavLinkCommunicator class package sending method
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 to send a packet, we need to form a message and write it into the packet. Let us assign this task to the already existing HeartbeatHandler class, and add the message sending signal to the AbstractHandler interface. The mavlink_msg_heartbeat_encode function writes the heartbeat message to the package, there are similar functions for all embedded messages. I’ll draw the reader’s attention that additional functions are also provided in mavlink, 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 sending function. More information on how to work with these features can be found here . The additional ending _chan (for example mavlink_msg_heartbeat_pack_chan) indicates which channel the message will be sent to.
The event code of the timerEvent class 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); } We will send heartbeat on a timer with a frequency of 1 Hz. If you put a debug output in the method of sending data to the communication channel data.toHex (), we will see our messages, according to the picture given at the beginning of the article. Each clock count should increase, and the checksum will change accordingly.
"fe09000100000821ee85017f0000023f08" "fe09010100000821ee85017f000002d576" "fe09020100000821ee85017f000002ebf5" In order to check whether our heartbeat is working, we will create two assembly targets: gcs - ground control station simulator and uav - drone simulator.
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 of this example can be found on github . I hope it was interesting, even though the first part came out pretty simple. In the next article I will try to tell you about other types of messages and what interesting things you can do with them. Thank you for attention!
Interesting links:
MAVLink official website
Dronecode project site
Tutorial in English from the site DIY Drones
Source: https://habr.com/ru/post/312300/
All Articles