|
|
|
@ -105,6 +105,8 @@ AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
@@ -105,6 +105,8 @@ AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
|
|
|
|
|
_gps_data_good(false) |
|
|
|
|
{ |
|
|
|
|
gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob)); |
|
|
|
|
// this guarantees that _term is always nul terminated
|
|
|
|
|
memset(_term, 0, sizeof(_term)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AP_GPS_NMEA::read(void) |
|
|
|
@ -198,19 +200,19 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
@@ -198,19 +200,19 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
|
|
|
|
|
int32_t ret = 0; |
|
|
|
|
|
|
|
|
|
// scan for decimal point or end of field
|
|
|
|
|
for (p = _term; isdigit(*p); p++) |
|
|
|
|
for (p = _term; *p && isdigit(*p); p++) |
|
|
|
|
; |
|
|
|
|
q = _term; |
|
|
|
|
|
|
|
|
|
// convert degrees
|
|
|
|
|
while ((p - q) > 2) { |
|
|
|
|
while ((p - q) > 2 && *q) { |
|
|
|
|
if (deg) |
|
|
|
|
deg *= 10; |
|
|
|
|
deg += DIGIT_TO_VAL(*q++); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// convert minutes
|
|
|
|
|
while (p > q) { |
|
|
|
|
while (p > q && *q) { |
|
|
|
|
if (min) |
|
|
|
|
min *= 10; |
|
|
|
|
min += DIGIT_TO_VAL(*q++); |
|
|
|
@ -220,7 +222,7 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
@@ -220,7 +222,7 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
|
|
|
|
|
if (*p == '.') { |
|
|
|
|
q = p + 1; |
|
|
|
|
float frac_scale = 0.1f; |
|
|
|
|
while (isdigit(*q)) { |
|
|
|
|
while (*q && isdigit(*q)) { |
|
|
|
|
frac_min += (*q++ - '0') * frac_scale; |
|
|
|
|
frac_scale *= 0.1f; |
|
|
|
|
} |
|
|
|
|