diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 7284e1f51f..614170a832 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -185,20 +185,22 @@ int16_t AP_GPS_NMEA::_from_hex(char a) return a - '0'; } -int32_t AP_GPS_NMEA::_parse_decimal_100() +int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p) { - char *p = _term; int32_t ret = 100UL * atol(p); int32_t sign = 1; + if (*p == '+') { ++p; } else if (*p == '-') { ++p; sign = -1; } + while (isdigit(*p)) { ++p; } + if (*p == '.') { if (isdigit(p[1])) { ret += sign * 10 * DIGIT_TO_VAL(p[1]); @@ -210,6 +212,7 @@ int32_t AP_GPS_NMEA::_parse_decimal_100() } } } + return ret; } @@ -390,14 +393,14 @@ bool AP_GPS_NMEA::_term_complete() _new_satellite_count = atol(_term); break; case _GPS_SENTENCE_GGA + 8: // HDOP (GGA) - _new_hdop = _parse_decimal_100(); + _new_hdop = (uint16_t)_parse_decimal_100(_term); break; // time and date // case _GPS_SENTENCE_RMC + 1: // Time (RMC) case _GPS_SENTENCE_GGA + 1: // Time (GGA) - _new_time = _parse_decimal_100(); + _new_time = _parse_decimal_100(_term); break; case _GPS_SENTENCE_RMC + 9: // Date (GPRMC) _new_date = atol(_term); @@ -424,18 +427,18 @@ bool AP_GPS_NMEA::_term_complete() _new_longitude = -_new_longitude; break; case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA) - _new_altitude = _parse_decimal_100(); + _new_altitude = _parse_decimal_100(_term); break; // course and speed // case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC) case _GPS_SENTENCE_VTG + 5: // Speed (VTG) - _new_speed = (_parse_decimal_100() * 514) / 1000; // knots-> m/sec, approximiates * 0.514 + _new_speed = (_parse_decimal_100(_term) * 514) / 1000; // knots-> m/sec, approximiates * 0.514 break; case _GPS_SENTENCE_RMC + 8: // Course (GPRMC) case _GPS_SENTENCE_VTG + 1: // Course (VTG) - _new_course = _parse_decimal_100(); + _new_course = _parse_decimal_100(_term); break; } } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 8f4772372f..663a5de4aa 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -85,13 +85,13 @@ private: /// int16_t _from_hex(char a); - /// Parses the current term as a NMEA-style decimal number with - /// up to two decimal digits. + /// Parses the @p as a NMEA-style decimal number with + /// up to 3 decimal digits. /// - /// @returns The value expressed by the string in _term, + /// @returns The value expressed by the string in @p, /// multiplied by 100. /// - int32_t _parse_decimal_100(); + static int32_t _parse_decimal_100(const char *p); /// Parses the current term as a NMEA-style degrees + minutes /// value with up to four decimal digits.