Building a MAVLink system in C++ typically involves two sides: a ground control station (GCS) running on a desktop or laptop, and an onboard companion application on the drone. Both use the same generated MAVLink C headers but serve different roles.
The flowcharts below show the implementation steps for each side. The GCS initiates connections, sends commands, and displays telemetry. The drone-side application receives commands, streams sensor data, and relays between the autopilot and the companion computer.
GCS Implementation Flow (Desktop C++)
UDP / Serial / Radio link — bidirectional MAVLink message stream
Drone Onboard Implementation Flow (Companion C++)
Serial UART — autopilot ↔ companion computer
Full-system message flow
GCS ↔ (UDP/radio) ↔ Autopilot ↔ (serial) ↔ Companion app. The autopilot routes messages between the GCS and companion based on sysid/compid. The companion can also open a UDP port and talk directly to the GCS if a Wi-Fi link exists.
Below is a minimal C++ example showing the core parsing and heartbeat loop. This pattern is the same for both GCS and drone side — only the transport setup and message handling differ.
cpp
#include <mavlink/common/mavlink.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <thread>
#include <chrono>
class MavlinkNode {
int sock_;
uint8_t sysid_, compid_;
mavlink_system_t system_;
public:
MavlinkNode(uint8_t sysid, uint8_t compid)
: sysid_(sysid), compid_(compid) {
system_.sysid = sysid;
system_.compid = compid;
}
void send_heartbeat() {
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_heartbeat_pack(
sysid_, compid_, &msg,
MAV_TYPE_GCS, // or MAV_TYPE_ONBOARD_CONTROLLER
MAV_AUTOPILOT_INVALID,
0, 0, MAV_STATE_ACTIVE
);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
send(sock_, buf, len, 0); // send over UDP or serial
}
void parse_incoming(uint8_t* data, size_t length) {
mavlink_message_t msg;
mavlink_status_t status;
for (size_t i = 0; i < length; i++) {
if (mavlink_parse_char(MAVLINK_COMM_0, data[i],
&msg, &status)) {
handle_message(msg);
}
}
}
void handle_message(const mavlink_message_t& msg) {
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
// Vehicle discovered
break;
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t att;
mavlink_msg_attitude_decode(&msg, &att);
// att.roll, att.pitch, att.yaw
break;
}
case MAVLINK_MSG_ID_COMMAND_ACK: {
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&msg, &ack);
// ack.command, ack.result
break;
}
default: break;
}
}
void send_command(uint16_t target_sys, uint16_t target_comp,
uint16_t cmd, float p1 = 0, float p2 = 0,
float p3 = 0, float p4 = 0, float p5 = 0,
float p6 = 0, float p7 = 0) {
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_command_long_pack(
sysid_, compid_, &msg,
target_sys, target_comp,
cmd, 0,
p1, p2, p3, p4, p5, p6, p7
);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
send(sock_, buf, len, 0);
}
};Build system
Add the generated MAVLink headers to your include path. With CMake: `include_directories(${CMAKE_SOURCE_DIR}/generated/include)`. The headers are standalone — no library to link. You only need a C++11 compiler and a socket or serial library.
The key functions from the generated headers are: mavlink_parse_char() to decode incoming bytes into messages, mavlink_msg_*_pack() to encode outgoing messages, mavlink_msg_to_send_buffer() to serialize to bytes, and mavlink_msg_*_decode() to extract typed fields from a received message.
Threading considerations
Run the heartbeat emitter on a 1 Hz timer thread. Run the receive loop on its own thread so parsing never blocks command sending. Protect shared state (target sysid, telemetry cache) with a mutex or lock-free queue.