当前位置：首页 > news >正文

PIXHAWK（ardupilot4.52）上传航点的bug

news 2025/7/1 6:10:16

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

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

发现经纬度只有前几位能够对应上，后几位都对应不上，两个点之间相差了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 failedsend_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);return;}// guided or change-altif (current == 2) {// current = 2 is a flag to tell us this is a "guided mode"// waypoint and not for the missionresult = (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 neededhandle_change_alt_request(cmd);// verify we received the commandresult = MAV_MISSION_ACCEPTED;}send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);return;}// not a guided-mode reqestMissionItemProtocol *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_itemmav_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_INTis to pass the lat/lng in MISSION_ITEM_INT straight through, andfor MISSION_ITEM multiply by 1e7 here. We need an exception forany commands which use the x and y fields not aslatitude/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_tif (!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万倍，那么精度就丢失了。如：