4. MAVLink全局位置消息解析与坐标转换

任务说明

本案例解析 MAVLink 的 GLOBAL_POSITION_INT（msgid=33）消息，提取纬度、经度、海拔高度、相对高度，并把经纬度从 1e7 缩放整数转换为度分秒（DMS）格式显示。使用 pymavlink 完成消息编解码，无需真实飞控，构造一条报文即可验证。

环境与依赖

pip install pymavlink

消息字段与转换关系

完整代码

from pymavlink.dialects.v20 import ardupilotmega as mav
from pymavlink import mavutil

def to_dms(value, is_lat):
    """十进制度 -> 度分秒字符串，带半球标识"""
    hemi = ('N' if value >= 0 else 'S') if is_lat else ('E' if value >= 0 else 'W')
    v = abs(value)
    d = int(v)
    m_full = (v - d) * 60
    m = int(m_full)
    s = (m_full - m) * 60
    return f"{d}°{m:02d}′{s:05.2f}″{hemi}"

def parse_global_position_int(msg):
    """解析 GLOBAL_POSITION_INT，返回结构化字典"""
    lat = msg.lat / 1e7
    lon = msg.lon / 1e7
    alt = msg.alt / 1000.0
    rel_alt = msg.relative_alt / 1000.0
    return {
        "lat": lat, "lon": lon,
        "lat_dms": to_dms(lat, True),
        "lon_dms": to_dms(lon, False),
        "alt_m": alt, "relative_alt_m": rel_alt,
    }

# 构造一条 GLOBAL_POSITION_INT 报文（模拟飞控发送）
src = mav.MAVLink(None)
src.srcSystem = 1
src.srcComponent = 1
# 北京天安门附近: 39.9087N, 116.3975E, 海拔 45.5m, 相对高度 12.3m
out_msg = mav.MAVLink_global_position_int_message(
    time_boot_ms=123456,
    lat=int(39.9087 * 1e7),
    lon=int(116.3975 * 1e7),
    alt=int(45.5 * 1000),
    relative_alt=int(12.3 * 1000),
    vx=0, vy=0, vz=0, hdg=0,
)
buf = out_msg.pack(src)

# 接收端解析
rx = mav.MAVLink(None)
parsed = rx.parse_char(buf)
assert parsed is not None and parsed.get_type() == "GLOBAL_POSITION_INT"

info = parse_global_position_int(parsed)
print(f"纬度: {info['lat']:.7f}  ->  {info['lat_dms']}")
print(f"经度: {info['lon']:.7f}  ->  {info['lon_dms']}")
print(f"海拔高度: {info['alt_m']:.3f} m")
print(f"相对高度: {info['relative_alt_m']:.3f} m")

运行方式

python global_pos_demo.py

验证方法与通过条件

• 脚本无异常退出，且 assert 通过（说明报文被正确解析为 GLOBAL_POSITION_INT）。
• 纬度输出为 39.9087000，DMS 为 39°54′31.32″N；经度输出为 116.3975000，DMS 为 116°23′51.00″E。
• 海拔高度输出 45.500 m，相对高度输出 12.300 m。

以上数值由代码中给定的输入唯一确定，属于确定性预期输出。接入真实飞控时，将 rx.parse_char(buf) 替换为从串口/UDP 读取的字节流即可，解析逻辑不变。