1. MAVLink心跳包解析与状态监控

任务目标

实现一个最小可运行的 MAVLink 心跳包解析器：接收 MAVLink v2 帧，校验帧结构，识别 HEARTBEAT（msgid=0）消息，并输出无人机类型、自动驾驶仪类型、系统状态、组件状态四个字段。

环境与依赖

• Python 3.8+，仅使用标准库 struct、socket，无需安装 pymavlink。
• MAVLink v2 帧格式：STX(0xFD) | len | incompat | compat | seq | sysid | compid | msgid(3B) | payload | CRC(2B)。
• HEARTBEAT payload 共 9 字节：custom_mode(u32) | type(u8) | autopilot(u8) | base_mode(u8) | system_status(u8) | mavlink_version(u8)。

实现思路

完整代码

import struct, socket

# MAVLink v2 帧头长度（不含 STX 后 9 字节 + 3 字节 msgid）
HEADER_LEN = 10
CRC_EXTRA = {0: 50}  # HEARTBEAT 的 CRC_EXTRA

MAV_TYPE = {0:"GENERIC",2:"QUADROTOR",3:"COAXIAL",4:"HELICOPTER",10:"GROUND_ROVER",11:"SURFACE_BOAT"}
MAV_AUTOPILOT = {0:"GENERIC",3:"ARDUPILOTMEGA",12:"PX4"}
MAV_STATE = {0:"UNINIT",1:"BOOT",2:"CALIBRATING",3:"STANDBY",4:"ACTIVE",5:"CRITICAL",6:"EMERGENCY",7:"POWEROFF"}

def x25_crc(data):
    crc = 0xFFFF
    for b in data:
        t = b ^ (crc & 0xFF)
        t ^= (t << 4) & 0xFF
        crc = ((crc >> 8) ^ (t << 8) ^ (t << 3) ^ (t >> 4)) & 0xFFFF
    return crc

def parse_heartbeat(payload):
    # 小端：custom_mode(u32), type, autopilot, base_mode, system_status, mavlink_version
    custom_mode, mtype, autopilot, base_mode, sys_status, mav_ver = struct.unpack("<IBBBBB", payload[:9])
    return {
        "custom_mode": custom_mode,
        "type": MAV_TYPE.get(mtype, f"UNKNOWN({mtype})"),
        "autopilot": MAV_AUTOPILOT.get(autopilot, f"UNKNOWN({autopilot})"),
        "base_mode": base_mode,
        "system_status": MAV_STATE.get(sys_status, f"UNKNOWN({sys_status})"),
        "component_status": "ARMED" if base_mode & 0x80 else "DISARMED",
        "mavlink_version": mav_ver,
    }

def try_parse_frame(buf):
    """从 buf 中找一帧完整 MAVLink v2 帧，返回 (msgid, payload, 剩余buf) 或 None"""
    i = buf.find(b"\xfd")
    if i < 0 or len(buf) - i < HEADER_LEN:
        return None
    buf = buf[i:]
    plen = buf[1]
    total = 1 + HEADER_LEN + plen + 2
    if len(buf) < total:
        return None
    frame = buf[:total]
    msgid = frame[7] | (frame[8] << 8) | (frame[9] << 16)
    payload = frame[10:10 + plen]
    crc_recv = frame[10 + plen] | (frame[11 + plen] << 8)
    crc_calc = x25_crc(frame[1:10 + plen] + bytes([CRC_EXTRA.get(msgid, 0)]))
    if crc_calc != crc_recv:
        return ("CRC_ERR", msgid, None, buf[total:])
    return (msgid, payload, buf[total:])

def monitor(host="0.0.0.0", port=14550):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((host, port))
    print(f"listening on {host}:{port}")
    buf = b""
    while True:
        data, _ = sock.recvfrom(2048)
        buf += data
        while True:
            r = try_parse_frame(buf)
            if r is None:
                break
            msgid, payload, buf = r
            if msgid == "CRC_ERR":
                print("CRC error, frame dropped")
                continue
            if msgid == 0:
                hb = parse_heartbeat(payload)
                print(f"[HEARTBEAT] type={hb['type']} autopilot={hb['autopilot']} "
                      f"system_status={hb['system_status']} component_status={hb['component_status']}")

if __name__ == "__main__":
    monitor()

运行与验证

1. 保存为 mav_heartbeat.py，运行 python mav_heartbeat.py，监听 UDP 14550。
2. 用 SITL 或任意 MAVLink 发送端向本机 14550 发送 HEARTBEAT 帧；也可用下面片段构造一帧自测：

import struct
from mav_heartbeat import x25_crc, CRC_EXTRA
payload = struct.pack("<IBBBBB", 0, 2, 3, 0x80, 4, 3)  # QUADROTOR/ARDUPILOT/ARMED/ACTIVE
hdr = bytes([0xFD, len(payload), 0, 0, 0, 1, 1, 0, 0, 0])
crc = x25_crc(hdr[1:] + payload + bytes([CRC_EXTRA[0]]))
frame = hdr + payload + struct.pack("<H", crc)
import socket; socket.socket(socket.AF_INET, socket.SOCK_DGRAM).sendto(frame, ("127.0.0.1", 14550))

通过条件：控制台打印形如 [HEARTBEAT] type=QUADROTOR autopilot=ARDUPILOTMEGA system_status=ACTIVE component_status=ARMED 的行；若 CRC 不匹配则打印 CRC error 并丢弃该帧。