PIXHAWK（ardupilot4.52）上传航点的bug-EW帮帮网

起因是查看飞控日志时发现地面站上传的平行航线，在日志看到航线却并不是平行的。

然后对比了一下地面站上传的航点信息跟飞控读取到的航点信息

发现经纬度只有前几位能够对应上，后几位都对应不上，两个点之间相差了50公分。地面站工程师认为地面站上传的数据没问题，是飞控解析的问题。

经检测，地面站上传航点任务用的是 MISSION ITEM (39)，MISSION ITEM (39)，在2020年就已经启用了，都2025年了，为啥还要用已经废弃了5年的消息？搞不懂。

但是该消息为啥会被弃用呢？

让我们一起来看下飞控是怎么解析该消息的。

在上一章我学习到了飞控是在GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)函数中进行解析航点任务的。

//libraries\GCS_MAVLink\GCS_common.cpp
void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) ///解析航点的
{
    mavlink_mission_item_int_t mission_item_int;
    bool send_mission_item_warning = false;
    if (msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) {
        mavlink_mission_item_t mission_item;
        mavlink_msg_mission_item_decode(&msg, &mission_item); 
        MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, mission_item_int);
        if (ret != MAV_MISSION_ACCEPTED) {
            const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;
            send_mission_ack(msg, type, ret);
            return;
        }
        send_mission_item_warning = true;
    } else {
        mavlink_msg_mission_item_int_decode(&msg, &mission_item_int);//解码函数
        gcs().send_text(MAV_SEVERITY_CRITICAL,"ss%d %ld,%ld",msg_sum++,mavlink_msg_mission_item_int_get_x(&msg),mavlink_msg_mission_item_int_get_y(&msg));
    }
    const uint8_t current = mission_item_int.current;
    const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;

    if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) {
        struct AP_Mission::Mission_Command cmd = {};
        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
        if (result != MAV_MISSION_ACCEPTED) {
            //decode failed
            send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
            return;
        }
        // guided or change-alt
        if (current == 2) {
            // current = 2 is a flag to tell us this is a "guided mode"
            // waypoint and not for the mission
            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
                      : MAV_MISSION_ERROR) ;
        } else if (current == 3) {
            //current = 3 is a flag to tell us this is a alt change only
            // add home alt if needed
            handle_change_alt_request(cmd);

            // verify we received the command
            result = MAV_MISSION_ACCEPTED;
        }
        send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
        return;
    }

    // not a guided-mode reqest
    MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type);
    if (prot == nullptr) {
        send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED);
        return;
    }

    if (send_mission_item_warning) {
        prot->send_mission_item_warning();
    }

    if (!prot->receiving) {
        send_mission_ack(msg, type, MAV_MISSION_ERROR);
        return;
    }

    prot->handle_mission_item(msg, mission_item_int);
}

主要看这部分

//libraries\GCS_MAVLink\GCS_common.cpp
//void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
    if (msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) {
        mavlink_mission_item_t mission_item;
        mavlink_msg_mission_item_decode(&msg, &mission_item);
//    gcs().send_text(MAV_SEVERITY_CRITICAL,"%d %f,%f",msg_sum++,mavlink_msg_mission_item_get_x(&msg),mavlink_msg_mission_item_get_y(&msg));
//    gcs().send_text(MAV_SEVERITY_CRITICAL, "x:%ld,y:%ld",(int32_t)(1.0e7f*mission_item.x),(int32_t)(1.0e7f*mission_item.y)); //这个跟地面站读取的一致
    gcs().send_text(MAV_SEVERITY_CRITICAL, "x:%f,y:%f",(mission_item.x),(mission_item.y));
        MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, mission_item_int);
        if (ret != MAV_MISSION_ACCEPTED) {
            const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;
            send_mission_ack(msg, type, ret);
            return;
        }
        send_mission_item_warning = true;
    }

mavlink_msg_mission_item_decode(&msg, &mission_item);是将msg的Mavlink消息解析并存放到mission_item中，mission_item的定义为：

//ardupilot\build\CubeOrange\libraries\GCS_MAVLink\include\mavlink\v2.0\common\mavlink_msg_mission_item.h
typedef struct __mavlink_mission_item_t {
 float param1; /*<  PARAM1, see MAV_CMD enum*/
 float param2; /*<  PARAM2, see MAV_CMD enum*/
 float param3; /*<  PARAM3, see MAV_CMD enum*/
 float param4; /*<  PARAM4, see MAV_CMD enum*/
 float x; /*<  PARAM5 / local: X coordinate, global: latitude*/
 float y; /*<  PARAM6 / local: Y coordinate, global: longitude*/
 float z; /*<  PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame).*/
 uint16_t seq; /*<  Sequence*/
 uint16_t command; /*<  The scheduled action for the waypoint.*/
 uint8_t target_system; /*<  System ID*/
 uint8_t target_component; /*<  Component ID*/
 uint8_t frame; /*<  The coordinate system of the waypoint.*/
 uint8_t current; /*<  false:0, true:1*/
 uint8_t autocontinue; /*<  Autocontinue to next waypoint*/
 uint8_t mission_type; /*<  Mission type.*/
} mavlink_mission_item_t;

其中的xy就是经纬度，将经纬度单独输出看一下

从输出的数据可以看到，经纬度都有个规律是都只有7个数字，那是因为float数据只有7位精度。

从飞控读取到的航点信息确实是只有前6位数值是一致的。后面的数据为啥就不一致了呢？

继续看后面飞控的使用

 MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, mission_item_int);

mission_item用于这里了，MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT函数会将航点任务信息进一步使用。

D:\Ardupilot\4.5.1\4.5\git_ardupilot\ardupilot4.5.1\ardupilot\libraries\AP_Mission\AP_Mission.cpp
MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &packet,
        mavlink_mission_item_int_t &mav_cmd)
{
    // TODO: rename mav_cmd to mission_item_int
    // TODO: rename packet to mission_item
    mav_cmd.param1 = packet.param1;
    mav_cmd.param2 = packet.param2;
    mav_cmd.param3 = packet.param3;
    mav_cmd.param4 = packet.param4;
    mav_cmd.z = packet.z;
    mav_cmd.seq = packet.seq;
    mav_cmd.command = packet.command;
    mav_cmd.target_system = packet.target_system;
    mav_cmd.target_component = packet.target_component;
    mav_cmd.frame = packet.frame;
    mav_cmd.current = packet.current;
    mav_cmd.autocontinue = packet.autocontinue;
    mav_cmd.mission_type = packet.mission_type;

    /*
      the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT
      is to pass the lat/lng in MISSION_ITEM_INT straight through, and
      for MISSION_ITEM multiply by 1e7 here. We need an exception for
      any commands which use the x and y fields not as
      latitude/longitude.
     */
    if (!cmd_has_location(packet.command)) {
        mav_cmd.x = packet.x;
        mav_cmd.y = packet.y;

    } else {
         //these commands use x and y as lat/lon. We need to
        // multiply by 1e7 to convert to int32_t
        if (!check_lat(packet.x)) {
            return MAV_MISSION_INVALID_PARAM5_X;
        }
        if (!check_lng(packet.y)) {
            return MAV_MISSION_INVALID_PARAM6_Y;
        }
        mav_cmd.x = packet.x * 1.0e7f;
        mav_cmd.y = packet.y * 1.0e7f;
    }

    return MAV_MISSION_ACCEPTED;
}

可以看到经纬度最终都 *1.0e7，而经纬度都是float数据，这里放大了100万倍，那么精度就丢失了。如：