深圳app网站建设哪家好,广西桂林,网站开发网络公司,绿化公司和苗圃做网站最近在测试过程中发现在椭球高为负的地方#xff0c;地面站读取GPS_RAW_INT (24)消息中的alt高度竟然是正值。而消息中定义的alt并不是一个unsigned数据#xff0c;理论上是带有正负符号的。 查看gga的原始信息#xff1a;
$GPGGA,063718.40,3714.8533856,N,11845.9411766,…最近在测试过程中发现在椭球高为负的地方地面站读取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(ret0) //优化当数值为-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了 问题解决。
