How MAVLink is integrated inside a flight controller — from UART drivers to the command handler and telemetry scheduler.

The previous topic covered the GCS and companion-computer sides. This topic focuses on the autopilot itself — the flight controller firmware where MAVLink is deeply integrated with the control loop, sensor drivers, and safety logic.

Whether you are building custom firmware from scratch or extending PX4 / ArduPilot, the MAVLink layer inside a flight controller follows a consistent architecture: UART driver → parser → message router → handlers (command, parameter, mission, telemetry) → serializer → UART output.

Autopilot MAVLink Architecture

Flight Controller Firmware

1. UART / transport driver

Hardware abstraction layer reads/writes bytes from TELEM1, TELEM2, USB serial, or UDP (if Ethernet-equipped). Each port is a separate MAVLink channel.

2. MAVLink parser

mavlink_parse_char() on each incoming byte. Validates CRC, checks message signing if enabled. Outputs mavlink_message_t structs.

3. Message router

Routes messages by target sysid/compid. Forwards to other ports if needed (GCS ↔ companion). Drops messages not addressed to this system.

4. Handler modules

Dedicated handlers for commands (COMMAND_LONG → ACK), parameters (PARAM_SET → PARAM_VALUE), missions (upload/download state machine), and FTP.

5. Flight controller core

EKF, attitude controller, position controller, and mixer. Receives setpoints from mission engine or offboard commands. Produces actuator outputs.

6. Telemetry scheduler

Streams HEARTBEAT, ATTITUDE, GLOBAL_POSITION_INT, SYS_STATUS, etc. at requested rates. Manages per-channel rate tables to stay within bandwidth.

7. Serializer and transmit

mavlink_msg_to_send_buffer() encodes messages. Bytes are pushed into the UART TX FIFO or DMA buffer for each active port.

React Flow

TELEM1 → GCS (radio) · TELEM2 → Companion (serial) · USB → Debug / config

PX4 architecture

In PX4, the MAVLink layer lives in src/modules/mavlink/. The main class `Mavlink` manages one instance per UART. `MavlinkReceiver` parses and dispatches. `MavlinkStream` subclasses define each telemetry stream. The module subscribes to uORB topics (the internal pub/sub bus) and re-publishes them as MAVLink messages.

ArduPilot architecture

In ArduPilot, MAVLink is handled by the GCS_MAVLink library. `GCS_MAVLINK::update_receive()` parses bytes. `handleMessage()` dispatches by msgid. Telemetry is scheduled in `GCS::update_send()` with a token-bucket rate limiter per channel. Parameters are stored in AP_Param and served through the parameter protocol.

If you are writing a custom flight controller (not extending PX4 or ArduPilot), the flowchart above is your implementation checklist. The core loop typically runs at 400 Hz on the main thread, while MAVLink parsing and transmission run on a lower-priority thread or in a DMA interrupt context.

cpp

// Minimal autopilot-side MAVLink integration (bare-metal / RTOS)

#include <mavlink/common/mavlink.h>

static mavlink_system_t mavlink_system = { 1, MAV_COMP_ID_AUTOPILOT1 };

void mavlink_rx_task(void* uart_handle) {
    uint8_t byte;
    mavlink_message_t msg;
    mavlink_status_t status;

    while (true) {
        if (uart_read(uart_handle, &byte, 1) > 0) {
            if (mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)) {
                handle_incoming(msg);
            }
        }
    }
}

void handle_incoming(const mavlink_message_t& msg) {
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_COMMAND_LONG: {
        mavlink_command_long_t cmd;
        mavlink_msg_command_long_decode(&msg, &cmd);

        uint8_t result = MAV_RESULT_UNSUPPORTED;
        switch (cmd.command) {
        case MAV_CMD_COMPONENT_ARM_DISARM:
            result = set_armed(cmd.param1 > 0.5f)
                ? MAV_RESULT_ACCEPTED
                : MAV_RESULT_DENIED;
            break;
        case MAV_CMD_NAV_TAKEOFF:
            result = initiate_takeoff(cmd.param7)
                ? MAV_RESULT_ACCEPTED
                : MAV_RESULT_FAILED;
            break;
        }

        // Always send ACK
        mavlink_message_t ack_msg;
        uint8_t buf[MAVLINK_MAX_PACKET_LEN];
        mavlink_msg_command_ack_pack(
            mavlink_system.sysid, mavlink_system.compid,
            &ack_msg, cmd.command, result, 0, 0,
            msg.sysid, msg.compid
        );
        uint16_t len = mavlink_msg_to_send_buffer(buf, &ack_msg);
        uart_write(uart_handle, buf, len);
        break;
    }
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
        send_all_parameters();
        break;
    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
        mavlink_set_position_target_local_ned_t sp;
        mavlink_msg_set_position_target_local_ned_decode(&msg, &sp);
        set_offboard_setpoint(sp.x, sp.y, sp.z, sp.vx, sp.vy, sp.vz);
        break;
    }
    default: break;
    }
}

void telemetry_task(void* uart_handle) {
    while (true) {
        mavlink_message_t msg;
        uint8_t buf[MAVLINK_MAX_PACKET_LEN];
        uint16_t len;

        // Heartbeat — 1 Hz
        mavlink_msg_heartbeat_pack(
            mavlink_system.sysid, mavlink_system.compid, &msg,
            MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
            get_base_mode(), get_custom_mode(), get_system_state()
        );
        len = mavlink_msg_to_send_buffer(buf, &msg);
        uart_write(uart_handle, buf, len);

        // Attitude — 50 Hz (shown at 1 Hz here for simplicity)
        float roll, pitch, yaw;
        get_attitude(&roll, &pitch, &yaw);
        mavlink_msg_attitude_pack(
            mavlink_system.sysid, mavlink_system.compid, &msg,
            millis(), roll, pitch, yaw, 0, 0, 0
        );
        len = mavlink_msg_to_send_buffer(buf, &msg);
        uart_write(uart_handle, buf, len);

        vTaskDelay(pdMS_TO_TICKS(1000)); // simplified; real scheduler is rate-based
    }
}

Command handler pattern

Every COMMAND_LONG your autopilot receives must get a COMMAND_ACK reply — even if the result is UNSUPPORTED. A GCS that never receives an ACK will retry forever or declare the link dead. The ACK should include the original command ID and the requesting sysid/compid.

The telemetry scheduler is critical for bandwidth management. A typical radio link runs at 57600 baud (~5.7 KB/s). Streaming ATTITUDE at 50 Hz plus GLOBAL_POSITION_INT at 10 Hz plus SYS_STATUS at 1 Hz already consumes a significant fraction of that. The scheduler must honor per-message rate requests from the GCS (via MAV_CMD_SET_MESSAGE_INTERVAL) while respecting the channel capacity.

Parameter storage

Parameters are typically stored in non-volatile memory (EEPROM or flash). On boot, the autopilot loads them into RAM. The parameter protocol reads from RAM and writes to both RAM and NV storage. Use param_count and param_index fields to let the GCS track which parameters it has received during a full read.

The mission engine inside the autopilot stores uploaded waypoints in persistent storage, advances through them during flight, and reports the current item via MISSION_CURRENT. On reboot, the stored mission is preserved. The mission protocol state machine must handle re-request timeouts and partial uploads gracefully.

Multi-port routing

Most autopilots expose 2–3 MAVLink ports (TELEM1, TELEM2, USB). The router must forward messages between them: a command arriving on TELEM1 (from GCS) and addressed to compid 191 (companion) gets forwarded out TELEM2. Broadcast messages (target_system=0) are forwarded to all ports except the one they arrived on.