上传任务

将任务上传到车辆

下图展示了将任务上传到无人机的通信序列（假设所有作都成功）。

更详细地说，作业顺序如下：

1. GCS 会发送 MISSION_COUNT 包括需要上传的任务项总数。
   · GCS必须启动一个超时等待 ，等待无人机的响应（MISSION_REQUEST_INT）。
2. 无人机收到消息并回复 MISSION_REQUEST_INT 请求第一个任务项（seq==0）。
   · 无人机必须启动一个超时等待 ，等待 GCS 的 MISSION_ITEM_INT 响应。
3. GCS 接收 MISSION_REQUEST_INT 并以 MISSION_ITEM_INT 消息回复请求的任务项目。
4. 无人机和 GCS 重复 MISSION_REQUEST_INT/MISSION_ITEM_INT 循环，迭代序列直到所有项目上传（seq==count-1）。
5. 收到最后一项任务物品后，无人机会回复 MISSION_ACK，并发出指示任务上传完成/成功的 MAV_MISSION_ACCEPTED 类型。
   · 无人机应将新任务设置为当前任务，丢弃原始数据。
   · 无人机认为上传完成。
6. GCS 接收包含 MAV_MISSION_ACCEPTED 的 MISSION_ACK 表示作已完成。
   · GCS 应该存储 MISSION_ACK.opaque_id（上传计划的当前 ID），之后可以用它来检查计划变更 。

mavlink id 44 MISSION_COUNT

mavlink id 51 MISSION_REQUEST_INT

mavlink id 73 MISSION_ITEM_INT

mavlink id 47 MISSION_ACK

MAV_MISSION_TYPE

MAV_MISSION_RESULT

python测试脚本

仅发送一个航点类型的任务项到无人机

import time
import sys
from pymavlink import mavutil

def convert_lat_lon_to_int(lat, lon):
    """将纬度和经度转换为int32_t * 1E7的格式"""
    lat_int = int(lat * 1e7)
    lon_int = int(lon * 1e7)
    return lat_int, lon_int

def send_mission_upload(connection, target_system, target_component, mission_items):
    """
    上传任务到飞行器（符合Mavlink文档规范）
    :param connection: MAVLink连接对象
    :param target_system: 目标系统ID
    :param target_component: 目标组件ID
    :param mission_items: 任务项列表，每个任务项是一个包含参数的字典
    """
    # 发送MISSION_COUNT（任务项数量）
    count = len(mission_items)
    print(f"发送MISSION_COUNT: {count}个任务项")
    connection.mav.mission_count_send(
        target_system,
        target_component,
        count,
        mavutil.mavlink.MAV_MISSION_TYPE_MISSION  # 任务类型：飞行计划
    )

    # 等待并处理每个任务项
    for seq in range(count):
        print(f"等待MISSION_REQUEST_INT for seq={seq}")
        retries = 0
        max_retries = 5  # 根据文档：Retries (max): 5

        while retries < max_retries:
            # 根据文档：Timeout (default): 1500 ms
            msg = connection.recv_match(
                type='MISSION_REQUEST_INT',
                blocking=True,
                timeout=1.5  # 1500 ms
            )

            if msg is not None:
                # 确保请求的是正确的序列号
                if msg.seq != seq:
                    print(f"错误: 请求的序列号不匹配，期望{seq}，收到{msg.seq}")
                    return False
                break
            else:
                print(f"超时，未收到MISSION_REQUEST_INT (尝试 {retries+1}/{max_retries})")
                retries += 1

        if retries == max_retries:
            print("达到最大重试次数，任务上传失败")
            return False

        # 发送MISSION_ITEM_INT
        item = mission_items[seq]
        print(f"发送MISSION_ITEM_INT for seq={seq}")
        connection.mav.mission_item_int_send(
            target_system,
            target_component,
            seq,
            item['frame'],
            item['command'],
            item['current'],
            item['autocontinue'],
            item['param1'],
            item['param2'],
            item['param3'],
            item['param4'],
            item['param5'],
            item['param6'],
            item['param7'],
            mavutil.mavlink.MAV_MISSION_TYPE_MISSION
        )

    # 等待MISSION_ACK
    print("等待MISSION_ACK")
    max_retries = 5
    retries = 0
    while retries < max_retries:
        msg = connection.recv_match(
            type='MISSION_ACK',
            blocking=True,
            timeout=1.5
        )

        if msg is not None:
            if msg.type == mavutil.mavlink.MAV_MISSION_ACCEPTED:
                print("任务上传成功!")
                return True
            else:
                print(f"任务上传失败，错误代码: {msg.type}")
                return False
        else:
            print(f"超时，未收到MISSION_ACK (尝试 {retries+1}/{max_retries})")
            retries += 1

    print("达到最大重试次数，任务上传失败")
    return False

def main():
    # 配置连接（根据您的实际情况修改）
    # 选项1: 串口连接 (Linux)
    # connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)

    # 选项2: UDP连接 (QGroundControl默认)
    connection = mavutil.mavlink_connection('udp:127.0.0.1:14550')

    print("等待心跳信号...")
    try:
        # 等待心跳包（最多等待10秒）
        msg = connection.wait_heartbeat(timeout=10)
        print(f"心跳包已收到！系统ID: {msg.get_srcSystem()}, 组件ID: {msg.get_srcComponent()}")

        target_system = msg.get_srcSystem()
        target_component = msg.get_srcComponent()

        # 创建简单任务：一个航点
        # 纬度: 47.3667, 经度: 8.5500, 高度: 100.0米
        lat = 41.3667
        lon = 111.5500
        altitude = 50.0

        # 转换坐标
        lat_int, lon_int = convert_lat_lon_to_int(lat, lon)

        # 任务项
        mission_item = {
            'frame': mavutil.mavlink.MAV_FRAME_GLOBAL,
            'command': mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
            'current': 0,  # 0表示不是当前任务项
            'autocontinue': 1,  # 1表示自动继续到下一个任务项
            'param1': 0,  # 0秒停留
            'param2': 0,  # 0秒停留
            'param3': 0,  # 0秒停留
            'param4': 0,  # 航向
            'param5': lat_int,  # 纬度
            'param6': lon_int,  # 经度
            'param7': altitude  # 高度
        }

        mission_items = [mission_item]

        # 上传任务
        success = send_mission_upload(connection, target_system, target_component, mission_items)
        if success:
            print("任务上传成功!")
        else:
            print("任务上传失败!")

    except Exception as e:
        print(f"连接错误: {e}")
        sys.exit(1)

    # 关闭连接
    connection.close()
    print("连接已关闭")

if __name__ == '__main__':
    main()