Code Examples
This section provides complete application examples using the MAVLink SDK.
Complete Application Example
Here’s a complete application that connects to a vehicle, monitors telemetry, and performs basic operations:
#include <iostream>
#include <thread>
#include <chrono>
#include "mavlink_sdk.h"
#include "mavlink_command.h"
#include "vehicle.h"
class DroneController : public mavlinksdk::CMavlinkEvents
{
private:
bool m_connected = false;
bool m_armed = false;
public:
void OnConnected(const bool& connected) override
{
m_connected = connected;
std::cout << "Connected: " << connected << std::endl;
}
void OnHeartBeat_First(const mavlink_heartbeat_t& heartbeat) override
{
std::cout << "Vehicle detected! Type: " << (int)heartbeat.type << std::endl;
// Request parameters after connection
mavlinksdk::CMavlinkCommand::getInstance().requestParametersList();
}
void OnArmed(const bool& armed, const bool& ready_to_arm) override
{
m_armed = armed;
std::cout << "Armed: " << armed << ", Ready: " << ready_to_arm << std::endl;
}
void OnModeChanges(const uint32_t& custom_mode, const int& firmware_type,
const MAV_AUTOPILOT& autopilot) override
{
std::cout << "Mode: " << custom_mode << std::endl;
}
void OnStatusText(const std::uint8_t& severity, const std::string& status) override
{
std::cout << "Status [" << (int)severity << "]: " << status << std::endl;
}
void OnACK(const int& cmd, const int& result, const std::string& result_msg) override
{
std::cout << "ACK for CMD " << cmd << ": " << result_msg << std::endl;
}
bool isConnected() const { return m_connected; }
bool isArmed() const { return m_armed; }
};
int main()
{
DroneController controller;
mavlinksdk::CMavlinkSDK& sdk = mavlinksdk::CMavlinkSDK::getInstance();
mavlinksdk::CMavlinkCommand& cmd = mavlinksdk::CMavlinkCommand::getInstance();
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
// Connect to SITL
sdk.connectUDP("0.0.0.0", 14550);
sdk.start(&controller);
// Wait for connection
while (!controller.isConnected())
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
// Wait for vehicle to be ready
std::this_thread::sleep_for(std::chrono::seconds(2));
// Example: Set mode to GUIDED (mode 4 for ArduCopter)
cmd.doSetMode(4);
std::this_thread::sleep_for(std::chrono::seconds(1));
// Example: Arm the vehicle
cmd.doArmDisarm(true, false);
// Main loop - print telemetry
for (int i = 0; i < 100; ++i)
{
if (vehicle.isFCBConnected())
{
auto pos = vehicle.getMsgGlobalPositionInt();
auto att = vehicle.getMsgAttitude();
std::cout << "Position: "
<< pos.lat / 1e7 << ", "
<< pos.lon / 1e7 << ", "
<< pos.relative_alt / 1000.0 << "m" << std::endl;
std::cout << "Attitude: Roll=" << att.roll * 57.3
<< " Pitch=" << att.pitch * 57.3
<< " Yaw=" << att.yaw * 57.3 << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
// Cleanup
sdk.stop();
return 0;
}
Telemetry Monitoring Example
Example of monitoring various telemetry data:
void printTelemetry()
{
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
if (!vehicle.isFCBConnected()) {
std::cout << "Vehicle disconnected!" << std::endl;
return;
}
// Position
auto gps = vehicle.getMsgGlobalPositionInt();
std::cout << "Position: "
<< gps.lat / 1e7 << ", "
<< gps.lon / 1e7 << std::endl;
std::cout << "Altitude (rel): " << gps.relative_alt / 1000.0 << " m" << std::endl;
// Attitude
auto att = vehicle.getMsgAttitude();
std::cout << "Roll: " << att.roll * 57.3 << "°" << std::endl;
std::cout << "Pitch: " << att.pitch * 57.3 << "°" << std::endl;
std::cout << "Yaw: " << att.yaw * 57.3 << "°" << std::endl;
// Speed
auto hud = vehicle.getMsgVFRHud();
std::cout << "Groundspeed: " << hud.groundspeed << " m/s" << std::endl;
std::cout << "Airspeed: " << hud.airspeed << " m/s" << std::endl;
std::cout << "Throttle: " << hud.throttle << "%" << std::endl;
// Battery
auto sys = vehicle.getMsgSysStatus();
std::cout << "Battery: " << sys.voltage_battery / 1000.0 << " V" << std::endl;
std::cout << "Current: " << sys.current_battery / 100.0 << " A" << std::endl;
std::cout << "Remaining: " << (int)sys.battery_remaining << "%" << std::endl;
// GPS Quality
auto gps_raw = vehicle.getMSGGPSRaw();
std::cout << "GPS Fix: " << (int)gps_raw.fix_type << std::endl;
std::cout << "Satellites: " << (int)gps_raw.satellites_visible << std::endl;
// Status
std::cout << "Armed: " << vehicle.isArmed() << std::endl;
std::cout << "Flying: " << vehicle.isFlying() << std::endl;
std::cout << "Ready to Arm: " << vehicle.isReadyToArm() << std::endl;
// Home
auto home = vehicle.getMsgHomePosition();
std::cout << "Home: " << home.latitude / 1e7 << ", "
<< home.longitude / 1e7 << std::endl;
// Navigation
auto nav = vehicle.getMsgNavController();
std::cout << "WP Distance: " << nav.wp_dist << " m" << std::endl;
std::cout << "Alt Error: " << nav.alt_error << " m" << std::endl;
// Lidar altitude (if available)
if (vehicle.hasLidarAltitude()) {
auto lidar = vehicle.getLidarAltitude();
std::cout << "Lidar Alt: " << lidar.current_distance / 100.0 << " m" << std::endl;
}
}
System ID Filtering Example
Example of restricting messages to a specific vehicle:
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
// Only accept messages from system ID 1
vehicle.restrictMessageToSysID(1);
// Only accept messages from component ID 1 (autopilot)
vehicle.restrictMessageToCompID(1);
// Get current system/component ID of connected vehicle
int sysid = vehicle.getSysId();
int compid = vehicle.getCompId();
std::cout << "Connected to System ID: " << sysid
<< ", Component ID: " << compid << std::endl;
Distance Sensor Example
Example of accessing distance sensor data:
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
// Get downward-facing lidar (altitude)
auto lidar = vehicle.getLidarAltitude();
std::cout << "Altitude: " << lidar.current_distance << " cm" << std::endl;
// Get specific orientation
auto front = vehicle.getDistanceSensor(MAV_SENSOR_ROTATION_NONE); // Forward
auto back = vehicle.getDistanceSensor(MAV_SENSOR_ROTATION_YAW_180); // Backward
auto down = vehicle.getDistanceSensor(MAV_SENSOR_ROTATION_PITCH_270); // Down (lidar)
High Latency Mode Example
Example of handling high-latency satellite links:
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
int mode = vehicle.getHighLatencyMode();
// 0 = Normal mode
// MAVLINK_MSG_ID_HIGH_LATENCY = High latency v1
// MAVLINK_MSG_ID_HIGH_LATENCY2 = High latency v2
if (mode == MAVLINK_MSG_ID_HIGH_LATENCY2) {
auto hl2 = vehicle.getHighLatency2();
// Use high latency data
std::cout << "High latency mode active" << std::endl;
}
Guided Mode Position Tracking
Example of tracking guided mode position for altitude changes:
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
// Set current guided target (called internally when gotoGuidedPoint is used)
vehicle.setGuidedPoint(latitude, longitude, relative_altitude);
// Get position for altitude change commands
LOCATION_3D pos = vehicle.getPositionforChangeAltitude();
Message Timestamps Example
Example of tracking when messages were last received:
mavlinksdk::CVehicle& vehicle = mavlinksdk::CVehicle::getInstance();
// Get timestamp of last GPS message (microseconds)
uint64_t gps_time = vehicle.getMessageTime(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
// Check if message has been processed
uint16_t flags = vehicle.getProcessedFlag(MAVLINK_MSG_ID_ATTITUDE);
if (flags == MESSAGE_UNPROCESSED) {
// New data available
vehicle.setProcessedFlag(MAVLINK_MSG_ID_ATTITUDE, MESSAGE_PROCESSED);
}
Connection Examples
SITL (Software In The Loop)
// Connect to ArduPilot SITL default port
sdk.connectUDP("0.0.0.0", 14550);
MAVProxy
// Connect to MAVProxy output
sdk.connectUDP("127.0.0.1", 14550);
Pixhawk via USB
// Linux
sdk.connectSerial("/dev/ttyACM0", 115200, false);
// Or with dynamic port detection
sdk.connectSerial("/dev/ttyACM0", 115200, true);
Telemetry Radio
// 3DR Radio or similar at 57600 baud
sdk.connectSerial("/dev/ttyUSB0", 57600, false);
TCP Connection
// Connect to MAVLink router or proxy
sdk.connectTCP("192.168.1.100", 5760);
Thread Safety Notes
The SDK uses internal threading for message reception
Callbacks are invoked from the communication thread
Use appropriate synchronization when accessing shared data from callbacks
The singleton instances are thread-safe for access