2. MAVLink系统状态消息解析与显示

任务说明

本案例使用 Python 的 pymavlink 库解析一条 MAVLink SYS_STATUS（ID=1）消息，提取电池电压、电流、剩余电量百分比、传感器健康位掩码，并在控制台格式化输出。输入采用一条构造的 SYS_STATUS 报文，通过 MAVLink.decode() 完成解析，验证解析结果与字段含义。

环境与依赖

• Python 3.8+
• pip install pymavlink

关键字段说明

• voltage_battery：电池电压，单位 mV，65535 表示无效。
• current_battery：电池电流，单位 10mA，-1 表示无效。
• battery_remaining：剩余电量百分比，-1 表示无效。
• onboard_control_sensors_health：传感器健康位掩码，位为 1 表示该传感器健康。

完整代码

from pymavlink.dialects.v20 import common as mavlink2

# 传感器健康位定义（MAV_SYS_STATUS_SENSOR）
SENSOR_BITS = {
    0x01: "3D_GYRO", 0x02: "3D_ACCEL", 0x04: "3D_MAG",
    0x08: "ABSOLUTE_PRESSURE", 0x10: "DIFFERENTIAL_PRESSURE",
    0x20: "GPS", 0x40: "OPTICAL_FLOW", 0x80: "VISION_POSITION",
    0x100: "LASER_POSITION", 0x200: "EXTERNAL_GROUND_TRUTH",
    0x400: "ANGULAR_RATE_CONTROL", 0x800: "ATTITUDE_STABILIZATION",
    0x1000: "YAW_POSITION", 0x2000: "Z_ALTITUDE_CONTROL",
    0x4000: "XY_POSITION_CONTROL", 0x8000: "MOTOR_OUTPUTS",
    0x10000: "RC_RECEIVER", 0x20000: "3D_GYRO2", 0x40000: "3D_ACCEL2",
    0x80000: "3D_MAG2", 0x100000: "GEOFENCE", 0x200000: "AHRS",
    0x400000: "TERRAIN", 0x800000: "REVERSE_MOTOR",
    0x1000000: "LOGGING", 0x2000000: "BATTERY",
    0x4000000: "PROXIMITY", 0x8000000: "SATCOM",
    0x10000000: "PREARM_CHECK", 0x20000000: "OBSTACLE_AVOIDANCE",
}

def decode_sensors(health_mask):
    """返回健康与不健康的传感器名称列表"""
    healthy, unhealthy = [], []
    for bit, name in SENSOR_BITS.items():
        (healthy if health_mask & bit else unhealthy).append(name)
    return healthy, unhealthy

def parse_sys_status(msg):
    """解析 SYS_STATUS 消息并格式化输出"""
    v = msg.voltage_battery
    c = msg.current_battery
    r = msg.battery_remaining

    voltage_v = "无效" if v == 65535 else f"{v / 1000.0:.2f} V"
    current_a = "无效" if c == -1 else f"{c / 100.0:.2f} A"
    remain = "无效" if r == -1 else f"{r} %"

    healthy, unhealthy = decode_sensors(msg.onboard_control_sensors_health)

    print("===== SYS_STATUS 解析结果 =====")
    print(f"电池电压   : {voltage_v}")
    print(f"电池电流   : {current_a}")
    print(f"剩余电量   : {remain}")
    print(f"健康传感器 : {', '.join(healthy) if healthy else '无'}")
    print(f"异常传感器 : {', '.join(unhealthy) if unhealthy else '无'}")

def build_sample_message():
    """构造一条 SYS_STATUS 报文用于验证"""
    mav = mavlink2.MAVLink(None)
    msg = mavlink2.MAVLink_sys_status_message(
        onboard_control_sensors_present=0xFFFFFFFF,
        onboard_control_sensors_enabled=0xFFFFFFFF,
        onboard_control_sensors_health=0x0000001F,  # 前5个传感器健康
        load=250,
        voltage_battery=12600,   # 12.6 V
        current_battery=1500,    # 15.0 A
        battery_remaining=78,
        drop_rate_comm=0,
        errors_comm=0,
        errors_count1=0, errors_count2=0,
        errors_count3=0, errors_count4=0,
    )
    return msg.pack(mav)

if __name__ == "__main__":
    raw = build_sample_message()
    mav = mavlink2.MAVLink(None)
    parsed = mav.decode(raw)
    parse_sys_status(parsed)

运行方式

python sys_status_demo.py

验证方法与通过条件

• 运行后控制台应输出 5 行字段信息，标题为 ===== SYS_STATUS 解析结果 =====。
• 电压应显示 12.60 V，电流应显示 15.00 A，剩余电量应显示 78 %。
• 健康传感器应包含 3D_GYRO, 3D_ACCEL, 3D_MAG, ABSOLUTE_PRESSURE, DIFFERENTIAL_PRESSURE，异常传感器列出其余位。
• 若将 voltage_battery 改为 65535、current_battery 改为 -1、battery_remaining 改为 -1，对应字段应显示“无效”。

解析流程