3. MAVLink姿态消息解析与可视化

任务说明

本案例解析 MAVLink 的 ATTITUDE（ID=30）消息，提取 roll / pitch / yaw（弧度）与 rollspeed / pitchspeed / yawspeed（弧度/秒），并实时绘制姿态曲线。为保持最小可复现，这里用纯 Python + matplotlib 做实时曲线，数据源用 pymavlink 从 UDP 端口接收；若没有真实飞控，可用同一脚本内的模拟发送端产生 ATTITUDE 报文，验证解析与绘图链路。

环境与依赖

pip install pymavlink matplotlib

MAVLink 消息定义由 pymavlink 自带，无需额外下载 XML。

核心实现

下面脚本包含两部分：sender() 通过 UDP 持续发送 ATTITUDE 报文（模拟飞控），receiver() 接收并解析，把角度与角速度写入环形缓冲，用 matplotlib 实时刷新曲线。真实使用时把 sender 换成飞控的 UDP/TCP 输出即可，接收端逻辑不变。

import math, threading, time
from collections import deque
from pymavlink import mavutil
import matplotlib.pyplot as plt
import matplotlib.animation as animation

UDP = "udpin:127.0.0.1:14550"
SEND = "udpout:127.0.0.1:14550"

def sender():
    m = mavutil.mavlink_connection(SEND, source_system=1)
    t = 0.0
    while True:
        t += 0.05
        m.mav.attitude_send(
            int(time.time() * 1000) & 0xFFFFFFFF,
            0.3 * math.sin(t),          # roll
            0.2 * math.cos(t * 0.7),    # pitch
            0.5 * math.sin(t * 0.3),    # yaw
            0.3 * math.cos(t),          # rollspeed
            -0.14 * math.sin(t * 0.7),  # pitchspeed
            0.15 * math.cos(t * 0.3),   # yawspeed
        )
        time.sleep(0.05)

N = 200
buf = {k: deque([0.0] * N, maxlen=N) for k in
       ("roll", "pitch", "yaw", "rollspeed", "pitchspeed", "yawspeed")}

def receiver():
    m = mavutil.mavlink_connection(UDP)
    m.wait_heartbeat(timeout=10)
    while True:
        msg = m.recv_match(type="ATTITUDE", blocking=True)
        if msg is None:
            continue
        buf["roll"].append(msg.roll)
        buf["pitch"].append(msg.pitch)
        buf["yaw"].append(msg.yaw)
        buf["rollspeed"].append(msg.rollspeed)
        buf["pitchspeed"].append(msg.pitchspeed)
        buf["yawspeed"].append(msg.yawspeed)

threading.Thread(target=sender, daemon=True).start()
threading.Thread(target=receiver, daemon=True).start()

fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True)
fig.suptitle("MAVLink ATTITUDE")
lines = {
    "roll":  ax1.plot([], [], label="roll")[0],
    "pitch": ax1.plot([], [], label="pitch")[0],
    "yaw":   ax1.plot([], [], label="yaw")[0],
    "rollspeed":  ax2.plot([], [], label="rollspeed")[0],
    "pitchspeed": ax2.plot([], [], label="pitchspeed")[0],
    "yawspeed":   ax2.plot([], [], label="yawspeed")[0],
}
ax1.set_ylabel("angle (rad)"); ax1.legend(loc="upper right")
ax2.set_ylabel("rate (rad/s)"); ax2.set_xlabel("samples"); ax2.legend(loc="upper right")
for ax in (ax1, ax2):
    ax.set_ylim(-1.0, 1.0); ax.grid(True)

def update(_):
    for k, ln in lines.items():
        ln.set_data(range(N), list(buf[k]))
    return list(lines.values())

ani = animation.FuncAnimation(fig, update, interval=50, blit=False)
plt.show()

运行与验证

1. 直接运行脚本：python attitude_plot.py。脚本会先启动模拟发送端，再启动接收端，最后弹出窗口。
2. 观察窗口：上方子图应出现三条随时间变化的曲线（roll/pitch/yaw），下方子图出现三条角速度曲线，均围绕 0 上下波动，幅度与发送端正弦幅值一致（约 ±0.3、±0.2、±0.5）。
3. 若接入真实飞控：把 sender() 线程去掉，将 UDP 改为飞控实际输出地址（如 udpin:0.0.0.0:14550 或 tcp:192.168.x.x:5760），其余解析与绘图逻辑不变。
4. 判定通过：曲线持续刷新、无异常退出、数值范围与发送端设定一致，即说明 ATTITUDE 解析与可视化链路正常。

关键点

• recv_match(type="ATTITUDE") 只取目标消息，避免其他报文干扰。
• 字段单位均为弧度与弧度/秒，如需角度制可乘 180/math.pi。
• 环形缓冲 deque(maxlen=N) 保证绘图窗口固定长度，避免内存增长。