MAVLink 无人机自主控制脚本
基于 pymavlink 的全程自主任务：连接 → GPS → 解锁 → 起飞 → 前进 → 降落
MAVLink 协议
GPS + EKF 校验
GUIDED 模式
速度控制 (SET_POSITION_TARGET_LOCAL_NED)
自动降落 + 上锁
脚本概述
本脚本用于控制运行 ArduPilot / PX4 固件的无人机，通过 MAVLink 协议完成完整的自主任务：
连接 → GPS 等待 → EKF 位置估计 → GUIDED 模式 → 解锁 → 起飞 2 米 → 前进指定距离 → 降落
通过 基于 UDP 通信（默认端口 14550，适配 SITL 或机载转发）通过 包含完整的超时保护与错误处理 通过 适用于无人机编队、自主巡检等场景
核心函数解析
| 函数 | 功能说明 |
|---|---|
connect() | UDP 连接飞控，等待心跳确认 |
set_mode(master, mode) | 切换飞行模式（GUIDED / LAND / LOITER） |
wait_for_gps(master) | 等待 3D 定位 + ≥8 颗卫星 |
wait_for_position_estimate(master) | 等待 EKF 位置估计（解锁前必须） |
arm(master) | 发送解锁指令，确认电机怠速 |
takeoff(master, target_alt) | 起飞至指定高度（默认 2 米） |
send_velocity(master, vx, vy, vz, yaw_rate) | 速度控制（机身 NED 坐标系，关键移动函数） |
fly_forward(master, distance, speed) | 以速度 speed 前进 distance 米 |
land(master) | 切换 LAND 模式，等待上锁 |
核心控制详解：send_velocity()
def send_velocity(master, vx=0, vy=0, vz=0, yaw_rate=0):
master.mav.set_position_target_local_ned_send(
0, master.target_system, master.target_component,
mavutil.mavlink.MAV_FRAME_BODY_NED, # 机身坐标系 (前/右/下)
0b0000111111000111, # 仅使用速度字段
0, 0, 0, vx, vy, vz, 0, 0, 0, 0, yaw_rate
)
vx/vy/vz 单位：m/s (前进/右/下降)
yaw_rate 单位：rad/s
若所有速度为 0 → 无人机悬停
yaw_rate 单位：rad/s
若所有速度为 0 → 无人机悬停
MAV_FRAME_BODY_NED：速度指令相对于无人机自身朝向。
Type mask = 0b0000111111000111：忽略位置/加速度，仅使用速度设定值。
Type mask = 0b0000111111000111：忽略位置/加速度，仅使用速度设定值。
fly_forward() 与 distance=0 行为
def fly_forward(master, distance=50.0, speed=1):
duration = distance / speed
start = time.time()
while time.time() - start < duration:
send_velocity(master, vx=speed, vy=0, vz=0)
time.sleep(0.1)
for _ in range(20):
send_velocity(master, 0, 0, 0)
警告 若传入
distance = 0，则 duration = 0 / speed = 0 → while 条件立即不成立，无人机不会前进，直接进入停止逻辑。此行为适用于：
- 通过 测试悬停功能
- 通过 无横向移动的空中定点 loiter
- 通过 清除意外残留的速度指令
完整脚本代码 (guided_test.py)
#!/usr/bin/env python3
import time
from pymavlink import mavutil
def connect():
print(" Connecting...")
master = mavutil.mavlink_connection("udp:127.0.0.1:14550")
master.wait_heartbeat()
print(f"通过 Connected: system={master.target_system}, component={master.target_component}")
return master
def set_mode(master, mode):
print(f" Setting mode: {mode}")
master.set_mode_apm(mode)
time.sleep(2)
def wait_for_gps(master):
print(" Waiting for GPS fix...")
while True:
msg = master.recv_match(type="GPS_RAW_INT", blocking=True, timeout=1)
if msg:
print(f"GPS fix_type={msg.fix_type}, satellites={msg.satellites_visible}")
if msg.fix_type >= 3 and msg.satellites_visible >= 8:
print("通过 GPS OK")
return
time.sleep(1)
def get_altitude(master):
msg = master.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)
if msg:
return msg.relative_alt / 1000.0
return None
def wait_for_position_estimate(master):
print(" Waiting for EKF position estimate / home...")
start = time.time()
while time.time() - start < 60:
msg = master.recv_match(
type=["EKF_STATUS_REPORT", "GLOBAL_POSITION_INT", "HOME_POSITION", "STATUSTEXT"],
blocking=True, timeout=1
)
if not msg:
continue
if msg.get_type() == "GLOBAL_POSITION_INT":
alt = msg.relative_alt / 1000.0
print(f" Position estimate active, rel_alt={alt:.2f}m")
return
raise Exception("错误 No position estimate / home not ready")
def arm(master):
print(" Arming...")
while master.recv_match(blocking=False): pass
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0
)
start = time.time()
while time.time() - start < 15:
msg = master.recv_match(type=["HEARTBEAT"], blocking=True, timeout=1)
if msg and (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED):
print("通过 Armed")
return
raise Exception("错误 Failed to arm automatically")
def takeoff(master, target_alt=2.0):
print(f" Taking off to {target_alt}m")
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, target_alt
)
while True:
alt = get_altitude(master)
if alt and alt >= target_alt * 0.95:
print("通过 Target altitude reached")
break
time.sleep(0.2)
def send_velocity(master, vx=0, vy=0, vz=0, yaw_rate=0):
master.mav.set_position_target_local_ned_send(
0, master.target_system, master.target_component,
mavutil.mavlink.MAV_FRAME_BODY_NED,
0b0000111111000111,
0, 0, 0, vx, vy, vz, 0, 0, 0, 0, yaw_rate
)
def fly_forward(master, distance=50.0, speed=1):
print(f" Moving forward {distance}m at {speed}m/s")
duration = distance / speed
start = time.time()
while time.time() - start < duration:
send_velocity(master, vx=speed, vy=0, vz=0)
time.sleep(0.1)
print(" Stopping")
for _ in range(20):
send_velocity(master, 0, 0, 0)
time.sleep(0.1)
def land(master):
print(" Landing...")
master.set_mode_apm("LAND")
master.motors_disarmed_wait()
print(" Disarmed")
def main():
master = connect()
wait_for_gps(master)
time.sleep(5)
wait_for_position_estimate(master)
set_mode(master, "GUIDED")
time.sleep(2)
arm(master)
takeoff(master, target_alt=2.0)
time.sleep(2)
fly_forward(master, distance=10.0, speed=0.2)
time.sleep(1)
land(master)
print(" Done")
if __name__ == "__main__":
main()
警告 安全与注意事项
强制安全建议：
- 通过 起飞前确保 GPS 定位 ≥ 8 颗卫星 + 3D Fix
- 通过 确认 EKF 位置估计就绪（否则解锁会被 PreArm 拒绝）
- 通过 首次测试请移除螺旋桨，验证油门/方向响应
- 通过 始终准备遥控器手动接管（推荐 RC 接收机）
- 通过 超时保护：GPS 60 秒，位置估计 60 秒，解锁 15 秒
MAVLink 关键概念：
Type mask
MAV_FRAME_BODY_NED：速度指令基于机身（前进/右/下）HEARTBEAT：周期状态消息，指示系统活跃与解锁状态Type mask
0b0000111111000111：忽略位置/加速度，仅使用速度字段通过 测试结论
UDP 心跳成功，飞控连接稳定
GPS 3D Fix 且卫星数 ≥ 8
EKF 位置估计就绪
解锁 + 起飞至 2 米成功
前进 10 米 @ 0.2 m/s 完成
LAND 模式降落 + 自动上锁
distance=0 时悬停模式验证通过
GPS 3D Fix 且卫星数 ≥ 8
EKF 位置估计就绪
解锁 + 起飞至 2 米成功
前进 10 米 @ 0.2 m/s 完成
LAND 模式降落 + 自动上锁
distance=0 时悬停模式验证通过
适配平台：ArduPilot / PX4 · SITL 或 机载计算机 · UDP 14550
(c) 卓博泰科技 · MAVLink 自主飞行控制脚本
(c) 卓博泰科技 · MAVLink 自主飞行控制脚本