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

PIXHAWK（ardupilot4.52）NMEA的解析bug

web 2025/6/28 8:35:14

最近在测试过程中发现在椭球高为负的地方，地面站读取GPS_RAW_INT (24)消息中的alt高度竟然是正值。而消息中定义的alt并不是一个unsigned数据，理论上是带有正负符号的。

查看gga的原始信息：

$GPGGA,063718.40,3714.8533856,N,11845.9411766,E,4,35,0.5,-0.3237,M,0.000,M,1.4,0002*60

高程为-0.3237，高程确实为负值。

修改了一下代码，单独输出一下高程进行测。

//AP_GPS.cpp
void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{const Location &loc = location(0);float hacc = 0.0f;float vacc = 0.0f;float sacc = 0.0f;float undulation = 0.0;int32_t height_elipsoid_mm = 0;if (get_undulation(0, undulation)) {height_elipsoid_mm = loc.alt*10 - undulation*1000;}horizontal_accuracy(0, hacc);vertical_accuracy(0, vacc);speed_accuracy(0, sacc);gcs().send_text(MAV_SEVERITY_CRITICAL, "remaining=%ld!",loc.alt * 10UL); //单独输出高程mavlink_msg_gps_raw_int_send(chan,last_fix_time_ms(0)*(uint64_t)1000,status(0),          //解状态loc.lat,        // in 1E7 degreesloc.lng,        // in 1E7 degreesloc.alt * 10UL, // in mmget_hdop(0),get_vdop(0),ground_speed(0)*100,  // cm/sground_course(0)*100, // 1/100 degrees,num_sats(0), //搜星数量height_elipsoid_mm,   // Ellipsoid height in mmhacc * 1000,          // one-sigma standard deviation in mmvacc * 1000,          // one-sigma standard deviation in mmsacc * 1000,          // one-sigma standard deviation in mm/s0,                    // TODO one-sigma heading accuracy standard deviationgps_yaw_cdeg(0));
}

更新固件后，将gga输入，输出确实变成了正值的：320。

继续索源，直接到NMEA解析中查看。

//AP_GPS_NMEA.cpp// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{// handle the last term in a message/**省略****/
/**省略****/
/**省略****/case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)_new_altitude = _parse_decimal_100(_term); //这里将负值改成正值了！//   _new_altitude = _parse_decimal_100("-1.24"); //这里将负值改成正值了！//   gcs().send_text(MAV_SEVERITY_CRITICAL, "now=%s",_term);//    gcs().send_text(MAV_SEVERITY_CRITICAL, "now=%ld",_new_altitude);break;// course and speed//case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
/**省略****/
}

请注意这个函数！！

_new_altitude = _parse_decimal_100(_term); //这里将负值改成正值了！

经过测试，当_parse_decimal_100函数输入"-0.3237",输出是：320。

但是！当输入是"-1.24"时，输出是：-124。

测试结果确实是能区分正负符号的，但是为啥输入-0.3237时，输出是正值呢？

继续看_parse_decimal_100(_term)函数。

//AP_GPS_NMEA.cpp
int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
{char *endptr = nullptr;long ret = 100 * strtol(p, &endptr, 10);  //提取到小数点之前的数字int sign = ret < 0 ? -1 : 1;//    gcs().send_text(MAV_SEVERITY_CRITICAL, "ret1=%ld",ret);if (ret >= (long)INT32_MAX) {return INT32_MAX;}if (ret <= (long)INT32_MIN) {return INT32_MIN;}if (endptr == nullptr || *endptr != '.') {return ret;}if (isdigit(endptr[1])) {  //提取小数点后的数字ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);if (isdigit(endptr[2])) {ret += sign * DIGIT_TO_VAL(endptr[2]);if (isdigit(endptr[3])) {ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);}}}//   gcs().send_text(MAV_SEVERITY_CRITICAL, "ret2=%ld",ret);return ret;
}

将该函数单独拎出来进行测试，输入-0.3237的结果为：

输入-1.3237的结果为：

问题出现在前端，前端用到了strtol函数，strtol函数是将字符串转成长整数，是能够区分正负符号的，但仅提取小数点之前的数值。

那么问题就来了，当小数点之前的数值为0时，0是不分正负的！

所以就导致了输入-0.3237，输出变成了正值的32。

在代码中可以看到有定义一个sign变量进行正负判断，那么现在多加一层判断弥补当小数点之前为0的情况。

//AP_GPS_NMEA.cpp
int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
{char *endptr = nullptr;long ret = 100 * strtol(p, &endptr, 10);  //提取到小数点之前的数字int sign = ret < 0 ? -1 : 1;if(ret==0) //优化当数值为-0.1的情况{if(atof(p)<-0.00001)sign=-1;}//    gcs().send_text(MAV_SEVERITY_CRITICAL, "ret1=%ld",ret);if (ret >= (long)INT32_MAX) {return INT32_MAX;}if (ret <= (long)INT32_MIN) {return INT32_MIN;}if (endptr == nullptr || *endptr != '.') {return ret;}if (isdigit(endptr[1])) {  //提取小数点后的数字ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);if (isdigit(endptr[2])) {ret += sign * DIGIT_TO_VAL(endptr[2]);if (isdigit(endptr[3])) {ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);}}}//   gcs().send_text(MAV_SEVERITY_CRITICAL, "ret2=%ld",ret);return ret;
}

再次测试，输入-0.3237时，能够正常输出 -32了