diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index dacbae8982..7284e1f51f 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -185,17 +185,29 @@ int16_t AP_GPS_NMEA::_from_hex(char a) return a - '0'; } -uint32_t AP_GPS_NMEA::_parse_decimal_100() +int32_t AP_GPS_NMEA::_parse_decimal_100() { char *p = _term; - uint32_t ret = 100UL * atol(p); - while (isdigit(*p)) + 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 += 10 * DIGIT_TO_VAL(p[1]); - if (isdigit(p[2])) - ret += DIGIT_TO_VAL(p[2]); + ret += sign * 10 * DIGIT_TO_VAL(p[1]); + if (isdigit(p[2])) { + ret += sign * DIGIT_TO_VAL(p[2]); + if (isdigit(p[3])) { + ret += sign * (DIGIT_TO_VAL(p[3]) >= 5); + } + } } } return ret; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index c728d36c46..8f4772372f 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -91,7 +91,7 @@ private: /// @returns The value expressed by the string in _term, /// multiplied by 100. /// - uint32_t _parse_decimal_100(); + int32_t _parse_decimal_100(); /// Parses the current term as a NMEA-style degrees + minutes /// value with up to four decimal digits.