modbender ArduPilot 无人机控制 Skill
通过 pymavlink 连接并控制 ArduPilot 无人机 (如 CubeOrange 等)。
⚠️ 重要：起飞核心流程
起飞必须连续发送命令，不要等待！
# 1. 等待飞控稳定 (status=3)
while True:
msg = master.wait_heartbeat(timeout=3)
if msg and msg.system_status == 3:
break
# 2. 连续发送：ARM → GUIDED → TAKEOFF (不要等待!)
master.mav.command_long_send(1, 1, 400, 0, 1, 21196, 0, 0, 0, 0, 0) # ARM (force=21196)
mode_map = master.mode_mapping()
master.set_mode(mode_map['GUIDED']) # GUIDED
master.mav.command_long_send(1, 1, 22, 0, 0, 0, 0, 0, 0, 0, 5) # TAKEOFF 5m
# 3. 监控高度
for i in range(40):
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=0.5)
if msg:
alt = msg.relative_alt / 1000
if alt >= 4.5:
print('✅ 到达目标高度!')
break
关键点：
- 发送 ARM 后立即发送 GUIDED 和 TAKEOFF，不要等待
- 如果等待，飞控会重新上锁
- force=21196 是 ArduPilot 的 magic value
连接
from pymavlink import mavutil
master = mavutil.mavlink_connection('tcp:localhost:5762')
master.wait_heartbeat(timeout=10)
system_id = master.target_system # 通常是 1
component_id = master.target_component
1. 检查状态
# 获取飞控状态
msg = master.wait_heartbeat(timeout=5)
print(f'status: {msg.system_status}') # 0=boot, 3=standby, 4=armed
# 获取高度
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=1)
print(f'高度: {msg.relative_alt / 1000}m')
# 获取 GPS
msg = master.recv_match(type='GPS_RAW_INT', timeout=1)
print(f'GPS: {msg.satellites_visible}颗, fix={msg.fix_type}')
# 获取电池
msg = master.recv_match(type='SYS_STATUS', timeout=1)
print(f'电池: {msg.voltage_battery / 1000}V')
2. 起飞 (关键流程)
# ⚠️ 必须等待飞控稳定 (status=3)
while True:
msg = master.wait_heartbeat(timeout=3)
if msg and msg.system_status == 3:
break
# ⚠️ 连续发送命令，不要等待!
master.mav.command_long_send(1, 1, 400, 0, 1, 21196, 0, 0, 0, 0, 0) # ARM
mode_map = master.mode_mapping()
master.set_mode(mode_map['GUIDED']) # GUIDED
master.mav.command_long_send(1, 1, 22, 0, 0, 0, 0, 0, 0, 0, 8) # TAKEOFF 8m
# 闭环监控
for i in range(40):
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=0.5)
if msg:
alt = msg.relative_alt / 1000
print(f'{i*0.5:.1f}s → {alt:.2f}m')
if alt >= 7.2: # 90% 目标
print('✅ 到达!')
break
3. 降落
# 切换到 LAND 模式
mode_map = master.mode_mapping()
master.set_mode(mode_map['LAND'])
# ⚠️ 必须持续发送 LAND 命令
for i in range(60):
master.mav.command_long_send(1, 1, 21, 0, 0, 0, 0, 0, 0, 0, 0)
import time
time.sleep(0.5)
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=0.3)
if msg:
alt = msg.relative_alt / 1000
if alt < 0.3:
print('✅ 降落完成!')
break
4. 相对移动 (LOCAL_POSITION_NED)
# 获取当前位置
local = master.recv_match(type='LOCAL_POSITION_NED', timeout=1)
# X轴前进2米 (NED: X=北)
master.mav.set_position_target_local_ned_send(
0, system_id, component_id,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b0000111111111000,
local.x + 2, local.y, local.z,
0, 0, 0, 0, 0, 0, 0, 0
)
坐标系 (NED)
- X轴: 北 (正=北)
- Y轴: 东 (正=东)
- Z轴: 向下 (负值=向上=高度)
完整示例
from pymavlink import mavutil
import time
master = mavutil.mavlink_connection('tcp:localhost:5762')
master.wait_heartbeat(timeout=10)
print('=== 起飞到 5m ===')
# 1. 等待飞控稳定
while True:
msg = master.wait_heartbeat(timeout=3)
if msg and msg.system_status == 3:
break
print('飞控就绪')
# 2. 连续发送: ARM → GUIDED → TAKEOFF
master.mav.command_long_send(1, 1, 400, 0, 1, 21196, 0, 0, 0, 0, 0)
mode_map = master.mode_mapping()
master.set_mode(mode_map['GUIDED'])
master.mav.command_long_send(1, 1, 22, 0, 0, 0, 0, 0, 0, 0, 5)
print('ARM + GUIDED + TAKEOFF')
# 3. 闭环监控
for i in range(40):
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=0.5)
if msg:
alt = msg.relative_alt / 1000
print(f'{i*0.5:.1f}s → {alt:.2f}m')
if alt >= 4.5:
print('✅ 5m!')
break
# === 降落 ===
mode_map = master.mode_mapping()
master.set_mode(mode_map['LAND'])
for i in range(60):
master.mav.command_long_send(1, 1, 21, 0, 0, 0, 0, 0, 0, 0, 0)
time.sleep(0.5)
msg = master.recv_match(type='GLOBAL_POSITION_INT', timeout=0.3)
if msg:
alt = msg.relative_alt / 1000
if alt < 0.3:
print('✅ 降落完成!')
break
注意事项
- 起飞：连续发送命令，不要等待
- 降落：持续发送 LAND 命令
- 闭环检查：每次操作前后获取状态
- GUIDED 模式：自主飞行必须用 GUIDED
- GPS：确保 fix_type >= 3
- 端口：常用 TCP 5762
依赖
pip install pymavlink