4. 第一个MAVLink程序：创建连接、发送与接收心跳包

好，到了真正动手的时候了。

前面三章我们聊了MAVLink是什么、协议怎么组织的、消息结构长什么样。说实话，光看理论容易犯困。我自己当年学的时候，也是看了半天文档，不如亲手跑通一个收发流程来得痛快。

这一章的目标很明确：写一个能跑的程序，创建MAVLink连接，发一个心跳包出去，再收一个回来，最后把数据解析出来。做完这一步，你就真正摸到MAVLink的门了。

4.1 选什么语言？用什么库？

MAVLink官方提供了C、Python、C++、Java等多种语言的代码生成工具。我个人最推荐初学者用Python + pymavlink起步。原因很简单：

• Python上手快，不用操心内存管理
• pymavlink封装得很完善，几行代码就能搞定连接
• 调试方便，出错了能快速定位

安装也简单，一行命令：

pip install pymavlink

嗯，这里要注意：pymavlink依赖future和lxml，如果安装报错，先装这两个依赖再试。我在Windows上遇到过lxml编译失败的情况，换成pip install lxml‑‑only‑binary就解决了。

4.2 创建MAVLink连接

连接方式取决于你的硬件场景。常见的有三种：

我个人习惯先用UDP做本地测试。你可以在电脑上跑一个MAVLink模拟器（比如mavproxy.py --master tcp:127.0.0.1:5760），然后地面站用UDP连上去。这样不用接硬件，调通了再换串口。

创建连接的代码长这样：

from pymavlink import mavutil

# 创建连接
master = mavutil.mavlink_connection('udp:0.0.0.0:14550')

# 等待心跳包，确认连接成功
master.wait_heartbeat()
print("连接成功！收到心跳包，系统ID:", master.target_system)

你想想看，wait_heartbeat()这步其实挺关键的。它不光确认了连接建立，还帮你拿到了对端飞控的系统ID和组件ID。后面发消息的时候，这两个ID必须填对。

4.3 发送心跳包

心跳包（HEARTBEAT）是MAVLink里最基础的消息，消息ID是0。它的作用很简单：告诉对方“我还活着”。

飞控会定期发心跳包给地面站。反过来，地面站也可以发心跳包给飞控，表明自己在线。有些飞控会检测地面站的心跳，如果超时没收到，就认为地面站掉线了，自动切换到失控保护模式。

发送心跳包的代码：

# 构造心跳包
master.mav.heartbeat_send(
    mavutil.mavlink.MAV_TYPE_GCS,      # 类型：地面站
    mavutil.mavlink.MAV_AUTOPILOT_INVALID,  # 自驾仪类型：无效
    0,                                  # 基础模式
    0,                                  # 自定义模式
    0                                   # 系统状态
)

print("心跳包已发送")

这里有个坑：MAV_TYPE_GCS表示你是地面站，不是飞控。如果你填了MAV_TYPE_QUADROTOR，飞控可能会把你当成另一架无人机，产生奇怪的行为。我曾经在测试时填错了类型，结果飞控一直尝试跟我做航点同步，日志刷得飞起。

4.4 接收心跳包

接收消息有两种方式：阻塞等待和非阻塞轮询。

阻塞方式：

# 阻塞等待，直到收到一个心跳包
msg = master.recv_match(type='HEARTBEAT', blocking=True)
print("收到心跳包！")

非阻塞方式：

# 非阻塞，没消息时返回None
msg = master.recv_match(type='HEARTBEAT', blocking=False)
if msg:
    print("收到心跳包！")
else:
    print("暂无消息")

实际项目中，我几乎不用阻塞方式。你想啊，地面站要同时处理遥测数据、指令响应、日志记录，如果卡在一个recv_match上，其他事情就全停了。正确的做法是在主循环里用非阻塞方式轮询，或者用多线程。

一个典型的主循环框架：

import time

while True:
    # 非阻塞接收所有消息
    msg = master.recv_match(blocking=False)
    if msg:
        # 根据消息类型分发处理
        if msg.get_type() == 'HEARTBEAT':
            print("收到心跳包")
        elif msg.get_type() == 'GPS_RAW_INT':
            print("GPS数据:", msg.lat, msg.lon)
        # ... 其他消息处理
    
    # 定期发送自己的心跳
    master.mav.heartbeat_send(
        mavutil.mavlink.MAV_TYPE_GCS,
        mavutil.mavlink.MAV_AUTOPILOT_INVALID,
        0, 0, 0
    )
    
    time.sleep(0.01)  # 10ms循环，别把CPU跑满

4.5 解析心跳包数据

收到心跳包之后，怎么把里面的数据拿出来？

心跳包的结构我们第三章讲过，包含类型、自驾仪类型、模式、状态等字段。在pymavlink里，消息对象直接以属性方式暴露这些字段：

# 假设msg是收到的HEARTBEAT消息
msg = master.recv_match(type='HEARTBEAT', blocking=True)

# 解析各个字段
system_type = msg.type              # 系统类型（飞控、地面站、相机等）
autopilot = msg.autopilot           # 自驾仪类型（ArduPilot、PX4等）
base_mode = msg.base_mode           # 基础模式（是否已武装、是否已解锁等）
custom_mode = msg.custom_mode       # 自定义模式（飞控特有的模式编号）
system_status = msg.system_status   # 系统状态（未初始化、已校准、飞行中）

print(f"系统类型: {system_type}")
print(f"自驾仪: {autopilot}")
print(f"基础模式: {bin(base_mode)}")  # 用二进制看模式标志位
print(f"自定义模式: {custom_mode}")
print(f"系统状态: {system_status}")

这里base_mode是一个位掩码，每一位代表一个开关状态。比如：

• bit 0: 是否已武装（ARMED）
• bit 1: 是否已解锁（UNLOCKED）
• bit 2: 是否在手动模式
• bit 3: 是否在自动模式
• bit 4: 是否在测试模式

你可以用位运算来判断：

if base_mode & 0b00000001:
    print("飞控已武装")
else:
    print("飞控未武装")

至于custom_mode，不同飞控的定义不一样。ArduPilot里，0是稳定模式，1是特技模式，2是悬停模式……PX4又是另一套编号。所以解析自定义模式时，你得知道对面跑的是什么固件。

4.6 完整示例：一个能跑的最小地面站

把上面的代码拼起来，就是一个最小可用的地面站程序：

import time
from pymavlink import mavutil

# 创建连接
master = mavutil.mavlink_connection('udp:0.0.0.0:14550')

# 等待飞控心跳
print("等待飞控心跳...")
master.wait_heartbeat()
print(f"连接成功！飞控系统ID: {master.target_system}")

# 主循环
while True:
    # 接收消息
    msg = master.recv_match(blocking=False)
    if msg:
        if msg.get_type() == 'HEARTBEAT':
            armed = msg.base_mode & 0b00000001
            print(f"心跳包 | 类型:{msg.type} 自驾仪:{msg.autopilot} 武装状态:{armed}")
    
    # 发送地面站心跳
    master.mav.heartbeat_send(
        mavutil.mavlink.MAV_TYPE_GCS,
        mavutil.mavlink.MAV_AUTOPILOT_INVALID,
        0, 0, 0
    )
    
    time.sleep(0.1)

跑起来之后，你会看到控制台每隔一秒左右打印一次心跳包信息。这说明你的地面站和飞控（或模拟器）已经成功建立了MAVLink通信。

4.7 本章知识体系

下面这张图帮你理清整个流程：

说白了，整个程序就三个核心动作：连接 → 收发 → 解析。连接是基础，收发是循环，解析是目的。你把这个三角关系搞清楚了，后面再复杂的MAVLink应用，无非是在这个框架上叠加更多的消息类型和处理逻辑。

好了，去试试吧。把代码跑起来，亲眼看到控制台打印出心跳包的那一刻，你会觉得前面三章的理论都没白学。