Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fix _parse_decimal_100() with negative numbers 

_parse_decimal_100() did not parse the fractional part for negative
numbers. Furthermore, use the third decimal (when present) for proper
rounding.
This commit is contained in:
Bert Regelink 2016-02-09 18:12:23 +01:00 committed by Lucas De Marchi
parent 0e09d0b996
commit 7379d120e1
2 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -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;

2
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -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.