|
|
|
@ -94,6 +94,7 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
@@ -94,6 +94,7 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
|
|
|
|
|
// Convenience macros //////////////////////////////////////////////////////////
|
|
|
|
|
//
|
|
|
|
|
#define DIGIT_TO_VAL(_x) (_x - '0') |
|
|
|
|
#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x)) |
|
|
|
|
|
|
|
|
|
AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : |
|
|
|
|
AP_GPS_Backend(_gps, _state, _port), |
|
|
|
@ -181,9 +182,9 @@ uint32_t AP_GPS_NMEA::_parse_decimal_100()
@@ -181,9 +182,9 @@ uint32_t AP_GPS_NMEA::_parse_decimal_100()
|
|
|
|
|
++p; |
|
|
|
|
if (*p == '.') { |
|
|
|
|
if (isdigit(p[1])) { |
|
|
|
|
ret += 10 * (p[1] - '0'); |
|
|
|
|
ret += 10 * DIGIT_TO_VAL(p[1]); |
|
|
|
|
if (isdigit(p[2])) |
|
|
|
|
ret += p[2] - '0'; |
|
|
|
|
ret += DIGIT_TO_VAL(p[2]); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
return ret; |
|
|
|
@ -223,7 +224,8 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
@@ -223,7 +224,8 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
|
|
|
|
|
q = p + 1; |
|
|
|
|
float frac_scale = 0.1f; |
|
|
|
|
while (*q && isdigit(*q)) { |
|
|
|
|
frac_min += (*q++ - '0') * frac_scale; |
|
|
|
|
frac_min += DIGIT_TO_VAL(*q) * frac_scale; |
|
|
|
|
q++; |
|
|
|
|
frac_scale *= 0.1f; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -375,8 +377,6 @@ bool AP_GPS_NMEA::_term_complete()
@@ -375,8 +377,6 @@ bool AP_GPS_NMEA::_term_complete()
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x)) |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
detect a NMEA GPS. Adds one byte, and returns true if the stream |
|
|
|
|
matches a NMEA string |
|
|
|
|