default·Friday, April 17, 2026 at 01:15:00·约 19 分钟读完

MAVLink 消息处理指南

MAVLink消息处理系统摘要本系统实现了一套完整的MAVLink消息处理机制，用于无人机通信与控制。核心功能包括：消息接收：通过UDP连接持续接收飞控消息，支持心跳维持机制消息处理：分类处理多种MAVLink消息类型，包括心跳、位置、姿态和航点相关消息航点管理：实现航点下载状态机，支持分片请求和接收航点数据消息转发：将处理后的消息通过MQTT发布到指定主题系统采用多线程架构，包含独立的消息接收线程和主处理循环，通过通道进行线程间通信。关键特性包括航点下载状态机、错误处理和日志记录机制，确保通信

rustmavlink

MAVLink 消息处理指南

本指南介绍项目中 MAVLink 消息的处理机制，包括消息接收、解析、转换和转发等核心功能。

HEARTBEAT：心跳消息，用于保持连接和获取系统状态
GLOBAL_POSITION_INT：全局位置信息，包括经纬度、高度和速度
ATTITUDE：姿态信息，包括横滚、俯仰和偏航角
MISSION_COUNT：航点数量消息
MISSION_ITEM_INT：航点详细信息
MISSION_REQUEST：航点请求消息
MISSION_ACK：航点操作确认消息
COMMAND_ACK：命令执行确认消息

消息接收

连接建立

系统通过以下方式建立与飞控的连接：

let conn_str = String::from("udpin:127.0.0.1:23445");
let conn = mavlink::connect::<MavMessage>(&conn_str).expect("连接失败");

接收循环

消息接收器运行在独立线程中，持续接收来自飞控的消息：

loop {
    // 定期发送心跳
    if last_heartbeat.elapsed() >= Duration::from_secs(1) {
        if let Err(e) = conn.send_default(&heartbeat_message()) {
            log::error!("心跳发送失败: {:?}", e);
        }
        last_heartbeat = std::time::Instant::now();
    }

    // 接收 MAVLink 消息
    match conn.recv() {
        Ok((header, msg)) => {
            // 转发消息到主循环
            if tx.blocking_send((header, msg.clone())).is_err() {
                log::warn!("主通道已关闭，退出接收线程");
                break;
            }

            // 发送消息到 Actor
            if let Err(e) = actor_tx.blocking_send(MavlinkActorMessage::MavlinkMessage((header, msg))) {
                log::error!("发送给 Actor 失败: {}", e);
                break;
            }
        }
        Err(e) => {
            log::error!("接收 MAVLink 消息失败: {:?}", e);
            thread::sleep(Duration::from_millis(100));
        }
    }
}

消息处理

主循环处理

主循环接收来自 MAVLink 接收器的消息，并根据消息类型进行不同处理：

while let Some((header, msg)) = mavlink_rx.recv().await {
    match msg {
        MavMessage::MISSION_COUNT(cnt) => {
            // 处理航点数量
            expected_total = cnt.count;
            waypoint_received.clear();
        }
        MavMessage::MISSION_ITEM_INT(item) => {
            // 处理航点信息
            let wp = Waypoint {
                seq: item.seq,
                lat: item.x as f64 / 1e7,
                lon: item.y as f64 / 1e7,
                alt: item.z,
            };
            waypoint_received.push(wp.clone());
        }
        MavMessage::MISSION_REQUEST(msg) => {
            // 处理航点请求
            is_uploading = true;
            let wp = WaypointWrite { seq: msg.seq };
        }
        MavMessage::MISSION_ACK(msg) => {
            // 处理航点确认
            if is_uploading {
                is_uploading = false;
                log::info!("航点上传完成");
            }
        }
        _ => {
            // 其他消息发布到 mavlink/incoming 频道
            if let Err(e) = send_mqtt_data(header, &msg, &client_clone, &topic_clone).await {
                log::error!("发送 MQTT 数据失败: {}", e);
            }
        }
    }
}

Actor 处理

MAVLink Actor 使用状态机模式处理消息，特别是航点相关的消息：

async fn handle_mavlink_message(&mut self, _header: MavHeader, msg: MavMessage) -> Result<()> {
    match msg {
        MavMessage::MISSION_COUNT(cnt) => {
            self.state = match self.state {
                MissionState::WaitingCount => {
                    if cnt.count == 0 {
                        MissionState::Idle
                    } else {
                        // 请求第一个航点
                        let req = MavMessage::MISSION_REQUEST_INT(MISSION_REQUEST_INT_DATA {
                            target_system: 1,
                            target_component: 1,
                            seq: 0,
                        });
                        if let Err(e) = self.vehicle.send_default(&req) {
                            log::error!("发送 MISSION_REQUEST_INT 失败: {}", e);
                            MissionState::Idle
                        } else {
                            MissionState::Downloading {
                                expected_count: cnt.count,
                                received: Vec::new(),
                            }
                        }
                    }
                }
                _ => self.state.clone(),
            };
        }
        MavMessage::MISSION_ITEM_INT(item) => {
            self.state = match std::mem::take(&mut self.state) {
                MissionState::Downloading {
                    expected_count,
                    mut received,
                } => {
                    let wp = Waypoint {
                        seq: item.seq,
                        lat: item.x as f64 / 1e7,
                        lon: item.y as f64 / 1e7,
                        alt: item.z,
                    };
                    received.push(wp);

                    let next_seq = item.seq + 1;
                    if next_seq < expected_count {
                        // 请求下一个航点
                        let req = MavMessage::MISSION_REQUEST_INT(MISSION_REQUEST_INT_DATA {
                            target_system: 1,
                            target_component: 1,
                            seq: next_seq,
                        });
                        if let Err(e) = self.vehicle.send_default(&req) {
                            log::error!("发送下一个 MISSION_REQUEST_INT 失败: {}", e);
                            MissionState::Idle
                        } else {
                            MissionState::Downloading {
                                expected_count,
                                received,
                            }
                        }
                    } else {
                        // 所有航点接收完毕
                        MissionState::Idle
                    }
                }
                _ => self.state.clone(),
            };
        }
        // ... 其他消息处理
        _ => {}
    }
    Ok(())
}

消息转换

JSON 转换

系统将 MAVLink 消息转换为 JSON 格式，便于通过 MQTT 传输和前端处理：

async fn send_mqtt_data(
    header: MavHeader,
    msg: &MavMessage,
    client: &AsyncClient,
    topic: &str,
) -> Result<()> {
    let payload = match msg {
        MavMessage::HEARTBEAT(data) => {
            let is_armed = data.base_mode.contains(MavModeFlag::MAV_MODE_FLAG_SAFETY_ARMED);
            let arm_status = if is_armed { "解锁" } else { "加锁" };

            json!({
                "header": {
                    "system_id": header.system_id,
                    "component_id": header.component_id,
                    "sequence": header.sequence,
                },
                "message_type": "HEARTBEAT",
                "data": {
                    "custom_mode": data.custom_mode,
                    "arm_status": arm_status,
                    "is_armed": is_armed,
                    // ... 其他字段
                }
            })
        }
        MavMessage::GLOBAL_POSITION_INT(data) => {
            json!({
                "header": {
                    "system_id": header.system_id,
                    "component_id": header.component_id,
                    "sequence": header.sequence,
                },
                "message_type": "GLOBAL_POSITION_INT",
                "data": {
                    "lat": data.lat as f64 / 1e7,
                    "lon": data.lon as f64 / 1e7,
                    "alt": data.alt as f32 / 1000.0,
                    "relative_alt": data.relative_alt as f32 / 1000.0,
                    "vx": data.vx as f32 / 100.0,
                    "vy": data.vy as f32 / 100.0,
                    "vz": data.vz as f32 / 100.0,
                    "hdg": data.hdg as f32 / 100.0,
                }
            })
        }
        MavMessage::ATTITUDE(data) => {
            json!({
                "header": {
                    "system_id": header.system_id,
                    "component_id": header.component_id,
                    "sequence": header.sequence,
                },
                "message_type": "ATTITUDE",
                "data": {
                    "roll": data.roll,
                    "pitch": data.pitch,
                    "yaw": data.yaw,
                    "rollspeed": data.rollspeed,
                    "pitchspeed": data.pitchspeed,
                    "yawspeed": data.yawspeed,
                }
            })
        }
        _ => {
            // 对于其他消息类型，使用通用转换
            let data_value = serde_json::to_value(msg)?;
            json!({
                "header": {
                    "system_id": header.system_id,
                    "component_id": header.component_id,
                    "sequence": header.sequence,
                },
                "message_type": msg.message_name(),
                "data": data_value,
            })
        }
    };

    // 发布到 MQTT
    let payload_str = serde_json::to_string(&payload)?;
    client.publish(topic, QoS::AtLeastOnce, false, payload_str.into_bytes()).await?;
    Ok(())
}

控制命令

加解锁控制

系统支持通过 MQTT 发送加解锁命令：

async fn handle_arm_disarm(&mut self, arm: bool) -> Result<()> {
    log::info!("收到解锁/上锁命令: {}", arm);
    let msg = MavMessage::COMMAND_LONG(COMMAND_LONG_DATA {
        target_system: 1,
        target_component: 1,
        command: MAV_CMD_COMPONENT_ARM_DISARM,
        confirmation: 1,
        param1: if arm { 1.0 } else { 0.0 },
        param2: 0.0,
        param3: 0.0,
        param4: 0.0,
        param5: 0.0,
        param6: 0.0,
        param7: 0.0,
    });
    if let Err(e) = self.vehicle.send_default(&msg) {
        log::error!("发送解锁/上锁命令失败: {}", e);
    }
    Ok(())
}

飞行模式切换

系统支持切换不同的飞行模式：

async fn handle_set_mode(&mut self, mode: String) -> Result<()> {
    log::info!("收到模式切换命令: {}", mode);
    let mode_value = match mode.as_str() {
        "MANUAL" => RoverMode::ROVER_MODE_MANUAL as u16,
        "ACRO" => RoverMode::ROVER_MODE_ACRO as u16,
        "STEERING" => RoverMode::ROVER_MODE_STEERING as u16,
        "HOLD" => RoverMode::ROVER_MODE_HOLD as u16,
        "LOITER" => RoverMode::ROVER_MODE_LOITER as u16,
        "FOLLOW" => RoverMode::ROVER_MODE_FOLLOW as u16,
        "SIMPLE" => RoverMode::ROVER_MODE_SIMPLE as u16,
        "DOCK" => RoverMode::ROVER_MODE_DOCK as u16,
        "CIRCLE" => RoverMode::ROVER_MODE_CIRCLE as u16,
        "AUTO" => RoverMode::ROVER_MODE_AUTO as u16,
        "RTL" => RoverMode::ROVER_MODE_RTL as u16,
        "SMART_RTL" => RoverMode::ROVER_MODE_SMART_RTL as u16,
        "GUIDED" => RoverMode::ROVER_MODE_GUIDED as u16,
        "INITIALISING" => RoverMode::ROVER_MODE_INITIALIZING as u16,
        _ => {
            log::warn!("未知的飞行模式: {}", mode);
            return Ok(());
        }
    };

    let msg = MavMessage::COMMAND_LONG(COMMAND_LONG_DATA {
        target_system: 1,
        target_component: 1,
        command: MAV_CMD_DO_SET_MODE,
        confirmation: 0,
        param1: 1.0,
        param2: mode_value as f32,
        param3: 0.0,
        param4: 0.0,
        param5: 0.0,
        param6: 0.0,
        param7: 0.0,
    });
    if let Err(e) = self.vehicle.send_default(&msg) {
        log::error!("发送模式切换命令失败: {}", e);
    };
    Ok(())
}

航点管理

下载航点列表

系统支持从飞控下载航点列表：

发送 MISSION_REQUEST_LIST 请求
接收 MISSION_COUNT 消息获取航点总数
逐个请求航点（MISSION_REQUEST_INT）
接收航点信息（MISSION_ITEM_INT）
完成后更新状态为 Idle

上传航点列表

系统支持向飞控上传航点列表：

发送 MISSION_CLEAR_ALL 清除现有航点
发送 MISSION_COUNT 告知航点总数
接收 MISSION_REQUEST 请求
逐个发送航点（MISSION_ITEM_INT）
接收 MISSION_ACK 确认

心跳机制

系统定期发送心跳消息以保持与飞控的连接：

pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage {
    mavlink::ardupilotmega::MavMessage::HEARTBEAT(mavlink::ardupilotmega::HEARTBEAT_DATA {
        custom_mode: 0,
        mavtype: mavlink::ardupilotmega::MavType::MAV_TYPE_QUADROTOR,
        autopilot: mavlink::ardupilotmega::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA,
        base_mode: mavlink::ardupilotmega::MavModeFlag::empty(),
        system_status: mavlink::ardupilotmega::MavState::MAV_STATE_STANDBY,
        mavlink_version: 0x3,
    })
}

常见问题

1. 消息丢失

如果出现消息丢失，请检查：

网络连接是否稳定
消息通道缓冲区是否足够大
是否有处理瓶颈导致消息积压

2. 航点上传失败

如果航点上传失败，请检查：

航点数据格式是否正确
飞控是否处于可接受航点上传的状态
是否有其他任务正在进行

3. 心跳超时

如果出现心跳超时，请检查：

飞控是否正常运行
网络连接是否正常
心跳发送间隔是否合理

下一篇rust 交叉编译指南

MAVLink 消息处理指南

MAVLink 消息处理指南

MAVLink 协议简介

消息类型

核心消息类型

消息接收

连接建立

接收循环

消息处理

主循环处理

Actor 处理

消息转换

JSON 转换

控制命令

加解锁控制

飞行模式切换

航点管理

下载航点列表

上传航点列表

心跳机制

常见问题

1. 消息丢失

2. 航点上传失败

3. 心跳超时