基于Pymavlink的任务规划实现

# QGroundControl 通常使用一个端口（示例：14540）
# MAVSDK-Python 连接使用另一个端口（示例：14550）
System().connect(system_address="udp://:14550")

# pymavlink 再使用第三个端口（示例：14600，或任意未占用端口）
PyMAVLinkWrapper("drone1")

# 关键点：不同应用使用不同UDP/TCP端口，避免端口冲突

PX4飞控
  ├─ 14540/udp ←→ QGroundControl
  ├─ 14550/udp ←→ MAVSDK-Python应用
  └─ 14600/udp ←→ pymavlink应用（或用户自定义端口/使用tcp）

waypoint = {
    'sequence': 0,           # 序列号
    'command': 16,          # MAV_CMD_NAV_WAYPOINT
    'frame': 3,             # MAV_FRAME_GLOBAL_RELATIVE_ALT
    'current': 0,           # 是否为当前航点
    'autocontinue': 1,      # 自动继续
    'param1': 0,            # 停留时间
    'param2': 5,            # 接受半径
    'param3': 0,            # 飞越半径
    'param4': 0,            # 航向角
    'param5': lat,          # 纬度
    'param6': lon,          # 经度
    'param7': alt,          # 高度
    'mission_type': 0       # 任务类型
}

# 创建发送器
sender = PyMAVLinkWrapper("drone1")

# 连接到飞控
await sender.connect("udp:127.0.0.1:14540")

# 创建正方形航点任务
side_length = 0.001  # 约100米
base_lat, base_lon, base_alt = 39.9042, 116.4074, 50.0  # 基准位置

# 第一个航点：基准位置
sender.add_waypoint(base_lat, base_lon, base_alt)
# 第二个航点：向东
sender.add_waypoint(base_lat + side_length, base_lon, base_alt)
# 第三个航点：东北
sender.add_waypoint(base_lat + side_length, base_lon + side_length, base_alt)
# 第四个航点：北
sender.add_waypoint(base_lat, base_lon + side_length, base_alt)

# 发送任务
success = await sender.send_mission()

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 自定义航点参数
sender.add_waypoint(
    lat=39.9042, 
    lon=116.4074, 
    alt=50.0, 
    acceptance_radius=10  # 接受半径10米
)

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 标准航点，默认命令16
sender.add_waypoint(lat=39.9042, lon=116.4074, alt=50.0)

# 返回起飞点，命令20
sender.add_waypoint(
    lat=0, lon=0, alt=0,  # 坐标会被忽略
    command=20,  # MAV_CMD_NAV_RETURN_TO_LAUNCH
    frame=3
)

# 降落命令，命令21
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=0,
    command=21,  # MAV_CMD_NAV_LAND
    frame=3,
    hold_time=10.0  # 中止高度10米
)

# 起飞命令，命令22
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=22,  # MAV_CMD_NAV_TAKEOFF
    frame=3,
    hold_time=15.0,  # 最小俯仰角15度
    yaw=90.0   # 偏航角90度
)

# 无限盘旋，命令84
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=84,  # MAV_CMD_NAV_LOITER_UNLIM
    frame=3,
    hold_time=50.0  # 盘旋半径50米
)

# 指定时间盘旋，命令86
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=86,  # MAV_CMD_NAV_LOITER_TIME
    frame=3,
    hold_time=60.0,  # 盘旋时间60秒
    acceptance_radius=30.0   # 盘旋半径30米
)

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 调查任务 - 多航点批量上传
survey_waypoints = [
    # 起飞点
    {"lat": 39.9042, "lon": 116.4074, "alt": 0, "command": 22, "hold_time": 0.0},
    # 调查点1
    {"lat": 39.9052, "lon": 116.4084, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 调查点2
    {"lat": 39.9062, "lon": 116.4094, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 调查点3
    {"lat": 39.9072, "lon": 116.4104, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 返回起飞点
    {"lat": 0, "lon": 0, "alt": 0, "command": 20, "hold_time": 0.0}
]

# 批量添加调查航点
for wp in survey_waypoints:
    sender.add_waypoint(
        lat=wp["lat"],
        lon=wp["lon"],
        alt=wp["alt"],
        command=wp["command"],
        hold_time=wp["hold_time"],
        acceptance_radius=5.0
    )

# 一次性上传所有调查航点
success = await sender.send_mission()

pip install pymavlink