A small and direct link to the flight controller.

I wrote a C++ component that lets a Raspberry Pi exchange data and commands with an ArduPilot flight controller. It reads each MAVLink message, checks it, then turns it into values the rest of the software can use. This is a very low-level task as it involves working with individual bits and creating a perfect communication protocol from scratch.
How it works
Visual of following one message

Flight controller
Sends telemetry Attitude, position, status
My C++ parser Waiting
01Find the message
02Check for damage
03Confirm the sender
04Translate the data
Application
Waiting
ATTITUDE
Roll: 0.0°
Pitch: 0.0°
Heading: 0.0°
No telemetry update yet.
This works the same in reverse.
Why I built it this way
Only the communication elements that the drone needed.

MAVLink usually comes with a large generated library. I implemented the required protocol handling directly to keep the companion software small and fast.
Technical details
MAVLink v2 frame structure

The parser reads the frame in order, validates it, then decodes supported messages into a thread-safe telemetry snapshot.
Field	Size	Purpose
Magic byte	1 byte	`0xFD` marks a MAVLink v2 frame
Header	9 bytes	Length, flags, sequence, source IDs, and 24-bit message ID
Payload	0 to 255 bytes	Message data, including zero-truncated payloads
Checksum	2 bytes	X.25 CRC with a message-specific extra byte
Signature	13 bytes, optional	Consumed when the signed-frame flag is set
Skills: C++, Linux serial I/O, Background receive thread, Source filtering, Telemetry decoding, Command transmission
Source code
Complete implementation

The header and implementation used by the visual above.
#include "mavlink_ctrl.h"

#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <cerrno>
#include <cstdio>
#include <cstring>
#include <cmath>
#include <stdexcept>
#include <fstream>
#include <iostream>
#include <chrono>
#include <vector>

// MAVLink v2 wire constants and message metadata
static constexpr uint8_t MAVLINK_STX_V2 = 0xFD;
static constexpr uint8_t MAV_SYS_ID = 1;
static constexpr uint8_t MAV_COMP_ID = 191;

static constexpr uint32_t MSG_ID_HEARTBEAT = 0;
static constexpr uint32_t MSG_ID_SET_POSITION_TARGET_LOCAL_NED = 84;

static constexpr uint8_t CRC_EXTRA_HEARTBEAT = 50;
static constexpr uint8_t CRC_EXTRA_SET_POSITION_TARGET_LOCAL_NED = 143;
static constexpr uint8_t CRC_EXTRA_REQUEST_DATA_STREAM = 148;
static constexpr uint8_t CRC_EXTRA_REMOTE_LOG_BLOCK_STATUS = 186;
static constexpr uint8_t CRC_EXTRA_COMMAND_LONG = 152;

static constexpr uint8_t ATT_TGT_IGNORE_BODY_ROLL_RATE = 0x01;
static constexpr uint8_t ATT_TGT_IGNORE_BODY_PITCH_RATE = 0x02;
static constexpr uint8_t ATT_TGT_IGNORE_BODY_YAW_RATE = 0x04;
static constexpr uint8_t ATT_TGT_IGNORE_ATTITUDE = 0x40;
static constexpr uint8_t ATT_TGT_IGNORE_THRUST = 0x80;

static constexpr uint32_t MSG_ID_REQUEST_DATA_STREAM = 66;
static constexpr uint32_t MSG_ID_REMOTE_LOG_BLOCK_STATUS = 185;
static constexpr uint32_t MSG_ID_COMMAND_LONG = 76;

static constexpr uint32_t MAV_REMOTE_LOG_DATA_BLOCK_STOP = 2147483645;
static constexpr uint32_t MAV_REMOTE_LOG_DATA_BLOCK_START = 2147483646;

static uint8_t get_crc_extra(uint32_t msg_id)
{
    switch (msg_id)
    {
    case 0:
        return 50;
    case 1:
        return 124;
    case 24:
        return 24;
    case 27:
        return 144;
    case 30:
        return 39;
    case 33:
        return 104;
    case 36:
        return 222;
    case 65:
        return 118;
    case 74:
        return 20;
    case 77:
        return 143;
    case 84:
        return 143;
    case 132:
        return 85;
    case 184:
        return 159;
    case 193:
        return 71;
    case 253:
        return 83;
    default:
        // Unknown message IDs cannot be checked without their CRC extra
        return 0;
    }
}

static void pack_float(uint8_t *dst, float val)
{
    uint32_t bits;
    std::memcpy(&bits, &val, 4);
    dst[0] = static_cast<uint8_t>(bits & 0xFF);
    dst[1] = static_cast<uint8_t>((bits >> 8) & 0xFF);
    dst[2] = static_cast<uint8_t>((bits >> 16) & 0xFF);
    dst[3] = static_cast<uint8_t>((bits >> 24) & 0xFF);
}

static void pack_u16(uint8_t *dst, uint16_t val)
{
    dst[0] = static_cast<uint8_t>(val & 0xFF);
    dst[1] = static_cast<uint8_t>((val >> 8) & 0xFF);
}

static void pack_u32(uint8_t *dst, uint32_t val)
{
    dst[0] = static_cast<uint8_t>(val & 0xFF);
    dst[1] = static_cast<uint8_t>((val >> 8) & 0xFF);
    dst[2] = static_cast<uint8_t>((val >> 16) & 0xFF);
    dst[3] = static_cast<uint8_t>((val >> 24) & 0xFF);
}

static float unpack_float(const uint8_t *p)
{
    uint32_t bits = static_cast<uint32_t>(p[0]) | (static_cast<uint32_t>(p[1]) << 8) | (static_cast<uint32_t>(p[2]) << 16) | (static_cast<uint32_t>(p[3]) << 24);
    float val;
    std::memcpy(&val, &bits, 4);
    return val;
}

static uint64_t unpack_u64(const uint8_t *p)
{
    uint64_t val = 0;
    for (int i = 7; i >= 0; i--)
        val = (val << 8) | p[i];
    return val;
}

static uint32_t unpack_u32(const uint8_t *p)
{
    return static_cast<uint32_t>(p[0]) | (static_cast<uint32_t>(p[1]) << 8) | (static_cast<uint32_t>(p[2]) << 16) | (static_cast<uint32_t>(p[3]) << 24);
}

static int32_t unpack_i32(const uint8_t *p)
{
    return static_cast<int32_t>(unpack_u32(p));
}

static uint16_t unpack_u16(const uint8_t *p)
{
    return static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8);
}

static int16_t unpack_i16(const uint8_t *p)
{
    return static_cast<int16_t>(unpack_u16(p));
}

// X.25 over the v2 header and payload, followed by the message CRC extra
static uint16_t mavlink_crc(const uint8_t *data, size_t len, uint8_t crc_extra)
{
    uint16_t crc = 0xFFFF;

    auto accumulate = [&crc](uint8_t byte)
    {
        uint8_t tmp = byte ^ (crc & 0xFF);
        tmp ^= (tmp << 4);
        crc = (crc >> 8) ^ (static_cast<uint16_t>(tmp) << 8) ^ (static_cast<uint16_t>(tmp) << 3) ^ (static_cast<uint16_t>(tmp) >> 4);
    };

    for (size_t i = 0; i < len; i++)
        accumulate(data[i]);
    accumulate(crc_extra);

    return crc;
}

static std::vector<uint8_t> build_mavlink_frame(uint8_t seq, uint32_t msg_id,
                                                const uint8_t *payload, uint8_t payload_len,
                                                uint8_t crc_extra)
{
    size_t frame_len = 10 + payload_len + 2;
    std::vector<uint8_t> frame(frame_len);

    frame[0] = MAVLINK_STX_V2;
    frame[1] = payload_len;
    frame[2] = 0;
    frame[3] = 0;
    frame[4] = seq;
    frame[5] = MAV_SYS_ID;
    frame[6] = MAV_COMP_ID;
    frame[7] = static_cast<uint8_t>(msg_id & 0xFF);
    frame[8] = static_cast<uint8_t>((msg_id >> 8) & 0xFF);
    frame[9] = static_cast<uint8_t>((msg_id >> 16) & 0xFF);

    if (payload_len > 0)
        std::memcpy(&frame[10], payload, payload_len);

    uint16_t crc = mavlink_crc(&frame[1], 9 + payload_len, crc_extra);
    frame[10 + payload_len] = static_cast<uint8_t>(crc & 0xFF);
    frame[11 + payload_len] = static_cast<uint8_t>((crc >> 8) & 0xFF);

    return frame;
}

uint64_t MAVLinkController::steady_us()
{
    using namespace std::chrono;
    return static_cast<uint64_t>(
        duration_cast<microseconds>(steady_clock::now().time_since_epoch()).count());
}

MAVLinkController::MAVLinkController(const std::string &serial_port, int baudrate)
    : port_(serial_port), baudrate_(baudrate)
{
}

MAVLinkController::~MAVLinkController()
{
    rx_running_.store(false);
    if (rx_thread_ && rx_thread_->joinable())
        rx_thread_->join();
    rx_thread_.reset();

    if (fd_ >= 0)
    {
        close(fd_);
        fd_ = -1;
    }
}

bool MAVLinkController::connect()
{
    fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (fd_ < 0)
    {
        std::cerr << "Failed to open serial port: " << port_ << std::endl;
        return false;
    }

    struct termios tty;
    std::memset(&tty, 0, sizeof(tty));

    if (tcgetattr(fd_, &tty) != 0)
    {
        std::cerr << "Failed to get terminal attributes" << std::endl;
        close(fd_);
        fd_ = -1;
        return false;
    }

    speed_t baud;
    switch (baudrate_)
    {
    case 57600:
        baud = B57600;
        break;
    case 115200:
        baud = B115200;
        break;
    case 230400:
        baud = B230400;
        break;
    case 460800:
        baud = B460800;
        break;
    case 921600:
        baud = B921600;
        break;
    default:
        baud = B921600;
        break;
    }
    cfsetispeed(&tty, baud);
    cfsetospeed(&tty, baud);

    cfmakeraw(&tty);

    tty.c_cflag &= ~PARENB;
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;
    tty.c_cflag &= ~CRTSCTS;
    tty.c_cflag |= CLOCAL | CREAD;

    tty.c_cc[VMIN] = 0;
    tty.c_cc[VTIME] = 0;

    if (tcsetattr(fd_, TCSANOW, &tty) != 0)
    {
        std::cerr << "Failed to set terminal attributes" << std::endl;
        close(fd_);
        fd_ = -1;
        return false;
    }

    tcflush(fd_, TCIOFLUSH);

    rx_running_.store(true);
    rx_thread_ = std::make_unique<std::thread>(&MAVLinkController::receive_loop, this);

    return true;
}

void MAVLinkController::send_raw(const uint8_t *data, size_t len)
{
    if (fd_ < 0)
        return;

    std::lock_guard<std::mutex> lock(tx_mutex_);

    size_t written = 0;
    int retries = 0;
    while (written < len)
    {
        ssize_t ret = write(fd_, data + written, len - written);
        if (ret < 0)
        {
            int err = errno;
            // Retry short-lived backpressure, but never block forever
            if ((err == EAGAIN || err == EWOULDBLOCK || err == EINTR) && retries++ < 100)
            {
                std::this_thread::sleep_for(std::chrono::microseconds(100));
                continue;
            }
            return;
        }
        written += static_cast<size_t>(ret);
        retries = 0;
    }
}

void MAVLinkController::set_tlog_callback(TlogCallback cb)
{
    tlog_cb_ = std::move(cb);
    tlog_cb_set_.store(true);
}

void MAVLinkController::send_velocity(const FlightCommand &cmd)
{
    uint8_t payload[53];
    std::memset(payload, 0, sizeof(payload));

    pack_u32(&payload[0], 0);
    pack_float(&payload[4], 0.0f);
    pack_float(&payload[8], 0.0f);
    pack_float(&payload[12], 0.0f);
    pack_float(&payload[16], cmd.vx);
    pack_float(&payload[20], cmd.vy);
    pack_float(&payload[24], cmd.vz);
    pack_float(&payload[28], 0.0f);
    pack_float(&payload[32], 0.0f);
    pack_float(&payload[36], 0.0f);
    pack_float(&payload[40], 0.0f);
    pack_float(&payload[44], cmd.yaw_rate * static_cast<float>(M_PI) / 180.0f);
    pack_u16(&payload[48], 0x07C7);
    payload[50] = 1;
    payload[51] = 1;
    payload[52] = 9;

    auto frame = build_mavlink_frame(seq_++, MSG_ID_SET_POSITION_TARGET_LOCAL_NED,
                                     payload, sizeof(payload),
                                     CRC_EXTRA_SET_POSITION_TARGET_LOCAL_NED);
    send_raw(frame.data(), frame.size());
}

void MAVLinkController::send_set_mode(uint8_t copter_mode, uint8_t target_sys, uint8_t target_comp)
{

    uint8_t payload[33];
    std::memset(payload, 0, sizeof(payload));

    pack_float(&payload[0], 1.0f);
    pack_float(&payload[4], static_cast<float>(copter_mode));
    pack_float(&payload[8], 0.0f);
    pack_float(&payload[12], 0.0f);
    pack_float(&payload[16], 0.0f);
    pack_float(&payload[20], 0.0f);
    pack_float(&payload[24], 0.0f);

    pack_u16(&payload[28], 176);
    payload[30] = target_sys;
    payload[31] = target_comp;
    payload[32] = 0;

    auto frame = build_mavlink_frame(seq_++, MSG_ID_COMMAND_LONG,
                                     payload, sizeof(payload),
                                     CRC_EXTRA_COMMAND_LONG);
    send_raw(frame.data(), frame.size());
}

void MAVLinkController::send_heartbeat()
{
    uint8_t payload[9];
    std::memset(payload, 0, sizeof(payload));

    pack_u32(&payload[0], 0);
    payload[4] = 18;
    payload[5] = 0;
    payload[6] = 0;
    payload[7] = 4;
    payload[8] = 2;

    auto frame = build_mavlink_frame(seq_++, MSG_ID_HEARTBEAT,
                                     payload, sizeof(payload),
                                     CRC_EXTRA_HEARTBEAT);
    send_raw(frame.data(), frame.size());
}

void MAVLinkController::request_data_streams(uint8_t target_sys, uint8_t target_comp, uint16_t rate_hz)
{
    uint8_t payload[6];

    auto send_stream = [&](uint8_t stream_id)
    {
        std::memset(payload, 0, sizeof(payload));
        pack_u16(&payload[0], rate_hz);
        payload[2] = target_sys;
        payload[3] = target_comp;
        payload[4] = stream_id;
        payload[5] = 1;
        auto frame = build_mavlink_frame(seq_++, MSG_ID_REQUEST_DATA_STREAM,
                                         payload, sizeof(payload),
                                         CRC_EXTRA_REQUEST_DATA_STREAM);
        send_raw(frame.data(), frame.size());
    };

    send_stream(0);
}

void MAVLinkController::send_arm_disarm(bool arm, bool force,
                                        uint8_t target_sys, uint8_t target_comp)
{
    uint8_t payload[33];
    std::memset(payload, 0, sizeof(payload));

    pack_float(&payload[0], arm ? 1.0f : 0.0f);
    pack_float(&payload[4], force ? 21196.0f : 0.0f);
    pack_u16(&payload[28], 400);
    payload[30] = target_sys;
    payload[31] = target_comp;
    payload[32] = 0;

    auto frame = build_mavlink_frame(seq_++, MSG_ID_COMMAND_LONG,
                                     payload, sizeof(payload),
                                     CRC_EXTRA_COMMAND_LONG);
    send_raw(frame.data(), frame.size());
}

void MAVLinkController::send_remote_log_block_status(uint32_t seqno, uint8_t status,
                                                     uint8_t target_sys, uint8_t target_comp)
{
    uint8_t payload[7];
    pack_u32(&payload[0], seqno);
    payload[4] = target_sys;
    payload[5] = target_comp;
    payload[6] = status;

    auto frame = build_mavlink_frame(seq_++, MSG_ID_REMOTE_LOG_BLOCK_STATUS,
                                     payload, sizeof(payload),
                                     CRC_EXTRA_REMOTE_LOG_BLOCK_STATUS);
    send_raw(frame.data(), frame.size());
}

void MAVLinkController::start_dataflash_stream(uint8_t target_sys, uint8_t target_comp)
{
    send_remote_log_block_status(MAV_REMOTE_LOG_DATA_BLOCK_START, 1, target_sys, target_comp);
}

void MAVLinkController::stop_dataflash_stream(uint8_t target_sys, uint8_t target_comp)
{
    send_remote_log_block_status(MAV_REMOTE_LOG_DATA_BLOCK_STOP, 1, target_sys, target_comp);
}

void MAVLinkController::set_dataflash_callback(DataflashCallback cb)
{
    std::lock_guard<std::mutex> lock(dataflash_mutex_);
    dataflash_cb_ = std::move(cb);
}

float MAVLinkController::read_altitude()
{
    std::lock_guard<std::mutex> lock(telem_mutex_);
    return static_cast<float>(telemetry_.relative_alt_mm) / 1000.0f;
}

float MAVLinkController::read_soc_temp()
{
    std::ifstream ifs("/sys/class/thermal/thermal_zone0/temp");
    if (!ifs.is_open())
        return -1.0f;
    int millideg = 0;
    ifs >> millideg;
    return static_cast<float>(millideg) / 1000.0f;
}

bool MAVLinkController::is_connected() const
{
    return fd_ >= 0;
}

Telemetry MAVLinkController::get_telemetry() const
{
    std::lock_guard<std::mutex> lock(telem_mutex_);
    return telemetry_;
}

// Byte-wise state machine for MAVLink v2 frames
void MAVLinkController::receive_loop()
{
    enum ParseState
    {
        WAIT_STX,
        READ_HEADER,
        READ_PAYLOAD,
        READ_CRC,
        READ_SIG
    };
    ParseState state = WAIT_STX;

    uint8_t header[9];
    int header_pos = 0;

    uint8_t payload[255];
    uint8_t payload_len = 0;
    int payload_pos = 0;

    uint8_t crc_bytes[2];
    int crc_pos = 0;
    int sig_pos = 0;

    uint8_t frame_buf[1 + 9 + 255 + 2 + 13];
    int frame_len = 0;

    uint8_t read_buf[512];

    int consecutive_errors = 0;
    while (rx_running_.load())
    {
        ssize_t n = read(fd_, read_buf, sizeof(read_buf));
        if (n <= 0)
        {
            int err = errno;
            // Serial disconnects remain recoverable without restarting the process
            if (n == 0 || err == EAGAIN || err == EWOULDBLOCK || err == EINTR ||
                err == EIO || err == ENXIO || err == ENODEV)
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
                consecutive_errors = 0;
                continue;
            }
            if (++consecutive_errors == 1 || consecutive_errors % 1000 == 0)
                std::cerr << "MAVLink serial read error (" << consecutive_errors
                          << "x): " << strerror(err) << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
        consecutive_errors = 0;

        for (ssize_t i = 0; i < n; i++)
        {
            uint8_t byte = read_buf[i];

            switch (state)
            {
            case WAIT_STX:
                if (byte == MAVLINK_STX_V2)
                {
                    frame_len = 0;
                    frame_buf[frame_len++] = byte;
                    header_pos = 0;
                    state = READ_HEADER;
                }
                break;

            case READ_HEADER:
                header[header_pos] = byte;
                frame_buf[frame_len++] = byte;
                header_pos++;
                if (header_pos == 9)
                {
                    payload_len = header[0];
                    payload_pos = 0;
                    state = (payload_len > 0) ? READ_PAYLOAD : READ_CRC;
                    crc_pos = 0;
                }
                break;

            case READ_PAYLOAD:
                if (payload_pos < 255)
                    payload[payload_pos] = byte;
                frame_buf[frame_len++] = byte;
                payload_pos++;
                if (payload_pos >= payload_len)
                {
                    crc_pos = 0;
                    state = READ_CRC;
                }
                break;

            case READ_CRC:
                crc_bytes[crc_pos] = byte;
                frame_buf[frame_len++] = byte;
                crc_pos++;
                if (crc_pos == 2)
                {
                    if (header[1] & 0x01)
                    {
                        sig_pos = 0;
                        state = READ_SIG;
                        break;
                    }
                    goto frame_complete;
                }
                break;

            case READ_SIG:
                // Signatures are consumed and logged, but not verified here
                frame_buf[frame_len++] = byte;
                sig_pos++;
                if (sig_pos < 13)
                    break;
            frame_complete:
            {
                uint32_t msg_id = static_cast<uint32_t>(header[6]) | (static_cast<uint32_t>(header[7]) << 8) | (static_cast<uint32_t>(header[8]) << 16);

                uint8_t extra = get_crc_extra(msg_id);
                if (extra != 0)
                {
                    uint8_t crc_data[9 + 255];
                    std::memcpy(crc_data, header, 9);
                    std::memcpy(crc_data + 9, payload, payload_len);

                    uint16_t computed = mavlink_crc(crc_data, 9 + payload_len, extra);
                    uint16_t received = static_cast<uint16_t>(crc_bytes[0]) | (static_cast<uint16_t>(crc_bytes[1]) << 8);

                    if (computed == received)
                    {
                        dispatch_message(msg_id, payload, payload_len,
                                         header[4], header[5]);

                        if (tlog_cb_set_.load())
                            tlog_cb_(frame_buf, frame_len);
                    }
                }

                state = WAIT_STX;
            }
            break;
            }
        }
    }
}

void MAVLinkController::dispatch_message(uint32_t msg_id, const uint8_t *raw, uint8_t len,
                                         uint8_t src_sys, uint8_t src_comp)
{
    // Ignore routed traffic from ground stations and other components
    if (src_sys != 1 || src_comp != 1)
        return;

    // MAVLink v2 may omit trailing zero bytes, so decode from a zero-filled copy
    uint8_t p[256];
    std::memset(p, 0, sizeof(p));
    if (len > 0)
        std::memcpy(p, raw, len);

    // Keep dataflash file I/O outside the telemetry lock
    if (msg_id == 184)
    {
        uint32_t seqno = unpack_u32(&p[0]);

        DataflashCallback cb;
        {
            std::lock_guard<std::mutex> dlock(dataflash_mutex_);
            cb = dataflash_cb_;
        }
        if (cb)
        {
            cb(seqno, &p[6], 200);
            send_remote_log_block_status(seqno, 1);
        }
        return;
    }

    std::lock_guard<std::mutex> lock(telem_mutex_);
    uint64_t now = steady_us();

    switch (msg_id)
    {

    case 0:
        telemetry_.custom_mode = unpack_u32(&p[0]);
        telemetry_.mav_type = p[4];
        telemetry_.autopilot_type = p[5];
        telemetry_.base_mode = p[6];
        telemetry_.system_status = p[7];
        telemetry_.armed = (p[6] & 0x80) != 0;
        telemetry_.ts_heartbeat = now;
        break;

    case 1:
        telemetry_.voltage_battery_mv = unpack_u16(&p[14]);
        telemetry_.current_battery_ca = unpack_i16(&p[16]);
        telemetry_.battery_remaining = static_cast<int8_t>(p[30]);
        telemetry_.ts_sys_status = now;
        break;

    case 24:
        telemetry_.gps_fix_type = p[28];
        telemetry_.satellites_visible = p[29];
        telemetry_.gps_eph = unpack_u16(&p[20]);
        telemetry_.gps_epv = unpack_u16(&p[22]);
        telemetry_.ts_gps_raw = now;
        break;

    case 27:
    {
        const float ax_g = unpack_i16(&p[8]) / 1000.0f;
        const float ay_g = unpack_i16(&p[10]) / 1000.0f;
        const float az_g = unpack_i16(&p[12]) / 1000.0f;
        telemetry_.accel_mag_g = std::sqrt(ax_g * ax_g + ay_g * ay_g + az_g * az_g);
        telemetry_.ts_raw_imu = now;
        break;
    }

    case 30:
        telemetry_.roll = unpack_float(&p[4]);
        telemetry_.pitch = unpack_float(&p[8]);
        telemetry_.yaw = unpack_float(&p[12]);
        telemetry_.rollspeed = unpack_float(&p[16]);
        telemetry_.pitchspeed = unpack_float(&p[20]);
        telemetry_.yawspeed = unpack_float(&p[24]);
        telemetry_.ts_attitude = now;
        break;

    case 33:
        telemetry_.lat = unpack_i32(&p[4]);
        telemetry_.lon = unpack_i32(&p[8]);
        telemetry_.alt_msl_mm = unpack_i32(&p[12]);
        telemetry_.relative_alt_mm = unpack_i32(&p[16]);
        telemetry_.vx = unpack_i16(&p[20]);
        telemetry_.vy = unpack_i16(&p[22]);
        telemetry_.vz = unpack_i16(&p[24]);
        telemetry_.hdg_cdeg = unpack_u16(&p[26]);
        telemetry_.ts_global_pos = now;
        break;

    case 36:
        for (int i = 0; i < 8; i++)
            telemetry_.servo_raw[i] = unpack_u16(&p[4 + i * 2]);
        telemetry_.ts_servo = now;
        break;

    case 65:
        for (int i = 0; i < 18; i++)
            telemetry_.rc_channels[i] = unpack_u16(&p[4 + i * 2]);
        telemetry_.rc_chancount = p[40];
        telemetry_.rc_rssi = p[41];
        telemetry_.ts_rc_channels = now;
        break;

    case 74:
        telemetry_.airspeed = unpack_float(&p[0]);
        telemetry_.groundspeed = unpack_float(&p[4]);
        telemetry_.baro_alt = unpack_float(&p[8]);
        telemetry_.climb = unpack_float(&p[12]);
        telemetry_.heading = unpack_i16(&p[16]);
        telemetry_.throttle = unpack_u16(&p[18]);
        telemetry_.ts_vfr_hud = now;
        break;

    case 132:
        // Only the downward rangefinder updates altitude above ground
        if (p[12] == 25)
        {
            telemetry_.rangefinder_max_cm = unpack_u16(&p[6]);
            telemetry_.rangefinder_cm = unpack_u16(&p[8]);
            telemetry_.ts_rangefinder = now;
        }
        break;

    case 193:
    {
        // GUIDED mode needs attitude and horizontal/vertical velocity health
        telemetry_.ekf_compass_variance = unpack_float(&p[12]);
        const uint16_t flags = unpack_u16(&p[20]);
        telemetry_.ekf_attitude_healthy = (flags & 0x0001) != 0;
        telemetry_.ekf_velocity_healthy = (flags & 0x0006) == 0x0006;
        telemetry_.ts_ekf_status = now;
        break;
    }

    case 253:
    {
        // Surface flight-controller warnings to stderr
        char text[51];
        std::memcpy(text, &p[1], 50);
        text[50] = '\0';
        std::fprintf(stderr, "[FC sev%u] %s\n", p[0], text);
        break;
    }

    default:
        break;
    }
}
#pragma once
#include <string>
#include <cstdint>
#include <functional>
#include <mutex>
#include <thread>
#include <atomic>
#include <memory>

struct FlightCommand {
    float vx, vy, vz;
    float yaw_rate;
};

// Thread-safe snapshot of telemetry decoded from the flight controller
struct Telemetry {
    uint32_t custom_mode       = 0;
    uint8_t  mav_type          = 0;
    uint8_t  autopilot_type    = 0;
    uint8_t  base_mode         = 0;
    uint8_t  system_status     = 0;
    bool     armed             = false;

    uint16_t voltage_battery_mv = 0;
    int16_t  current_battery_ca = 0;
    int8_t   battery_remaining  = -1;

    uint8_t  gps_fix_type       = 0;
    uint8_t  satellites_visible = 0;
    uint16_t gps_eph            = 0;
    uint16_t gps_epv            = 0;

    float    roll       = 0.0f;
    float    pitch      = 0.0f;
    float    yaw        = 0.0f;
    float    rollspeed  = 0.0f;
    float    pitchspeed = 0.0f;
    float    yawspeed   = 0.0f;

    int32_t  lat             = 0;
    int32_t  lon             = 0;
    int32_t  alt_msl_mm      = 0;
    int32_t  relative_alt_mm = 0;
    int16_t  vx              = 0;
    int16_t  vy              = 0;
    int16_t  vz              = 0;
    uint16_t hdg_cdeg        = 0;

    uint16_t servo_raw[8] = {};

    uint16_t rc_channels[18] = {};
    uint8_t  rc_chancount     = 0;
    uint8_t  rc_rssi          = 0;

    float    airspeed    = 0.0f;
    float    groundspeed = 0.0f;
    float    baro_alt    = 0.0f;
    float    climb       = 0.0f;
    int16_t  heading     = 0;
    uint16_t throttle    = 0;

    float    accel_mag_g = 0.0f;

    uint16_t rangefinder_cm     = 0;
    uint16_t rangefinder_max_cm = 0;

    bool     ekf_attitude_healthy   = false;
    bool     ekf_velocity_healthy   = false;
    float    ekf_compass_variance   = 999.0f;

    uint64_t ts_heartbeat   = 0;
    uint64_t ts_sys_status  = 0;
    uint64_t ts_gps_raw     = 0;
    uint64_t ts_attitude    = 0;
    uint64_t ts_global_pos  = 0;
    uint64_t ts_servo       = 0;
    uint64_t ts_rc_channels = 0;
    uint64_t ts_vfr_hud     = 0;
    uint64_t ts_raw_imu     = 0;
    uint64_t ts_rangefinder = 0;
    uint64_t ts_ekf_status  = 0;
};

// MAVLink v2 serial transport, parser, telemetry store, and command sender
class MAVLinkController {
public:
    MAVLinkController(const std::string& serial_port, int baudrate = 921600);
    ~MAVLinkController();

    bool connect();
    void send_velocity(const FlightCommand& cmd);
    void send_heartbeat();
    void request_data_streams(uint8_t target_sys = 1, uint8_t target_comp = 1, uint16_t rate_hz = 10);

    void send_set_mode(uint8_t copter_mode, uint8_t target_sys = 1, uint8_t target_comp = 1);
    void send_arm_disarm(bool arm, bool force = false, uint8_t target_sys = 1, uint8_t target_comp = 1);

    void send_remote_log_block_status(uint32_t seqno, uint8_t status,
                                       uint8_t target_sys = 1, uint8_t target_comp = 1);
    void start_dataflash_stream(uint8_t target_sys = 1, uint8_t target_comp = 1);
    void stop_dataflash_stream(uint8_t target_sys = 1, uint8_t target_comp = 1);

    using DataflashCallback = std::function<void(uint32_t seqno, const uint8_t* data, size_t len)>;
    void set_dataflash_callback(DataflashCallback cb);
    float read_altitude();
    float read_soc_temp();
    bool is_connected() const;

    Telemetry get_telemetry() const;

    using TlogCallback = std::function<void(const uint8_t* frame, size_t len)>;
    void set_tlog_callback(TlogCallback cb);

private:
    // Shared by the control and receive threads
    std::atomic<int> fd_{-1};
    std::string port_;
    int baudrate_;
    // Incremented by command sends and receive-thread acknowledgements
    std::atomic<uint8_t> seq_{0};

    std::atomic<bool> rx_running_{false};
    std::unique_ptr<std::thread> rx_thread_;

    mutable std::mutex telem_mutex_;
    Telemetry telemetry_;

    TlogCallback tlog_cb_;
    std::atomic<bool> tlog_cb_set_{false};

    mutable std::mutex dataflash_mutex_;
    DataflashCallback dataflash_cb_;

    std::mutex tx_mutex_;

    void send_raw(const uint8_t* data, size_t len);
    void receive_loop();
    // Only autopilot source 1/1 is allowed to update telemetry
    void dispatch_message(uint32_t msg_id, const uint8_t* payload, uint8_t len,
                          uint8_t src_sys, uint8_t src_comp);

    static uint64_t steady_us();
};