Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: improved the precision of the NMEA driver

Browse Source 
this brings the NMEA driver to the same lat/lon precision as the uBlox
driver (approx 1cm)
This commit is contained in:
Andrew Tridgell 2013-08-14 14:28:34 +10:00
parent 97baec8bc7
commit b43bf44552
1 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -185,11 +185,14 @@ uint32_t AP_GPS_NMEA::_parse_decimal()
return ret; return ret;
} }
/*
parse a NMEA latitude/longitude degree value. The result is in degrees*10e7
*/
uint32_t AP_GPS_NMEA::_parse_degrees() uint32_t AP_GPS_NMEA::_parse_degrees()
{ {
char *p, *q; char *p, *q;
uint8_t deg = 0, min = 0; uint8_t deg = 0, min = 0;
uint32_t frac_min = 0; float frac_min = 0;
int32_t ret = 0; int32_t ret = 0;
// scan for decimal point or end of field // scan for decimal point or end of field
@ -212,17 +215,17 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
} }
// convert fractional minutes // convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') { if (*p == '.') {
q = p + 1; q = p + 1;
for (int16_t i = 0; i < 5; i++) { float frac_scale = 0.1f;
frac_min = (int32_t)(frac_min * 10); while (isdigit(*q)) {
if (isdigit(*q)) frac_min += (*q++ - '0') * frac_scale;
frac_min += *q++ - '0'; frac_scale *= 0.1f;
} }
} }
ret = (int32_t)deg * (int32_t)1000000UL + (int32_t)((min * 100000UL + frac_min) / 6UL); ret = (deg * (int32_t)10000000UL);
ret += (min * (int32_t)10000000UL / 60);
ret += frac_min * (1.0e7 / 60.0f);
return ret; return ret;
} }
@ -239,8 +242,8 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_GPRMC:
time = _new_time; time = _new_time;
date = _new_date; date = _new_date;
latitude = _new_latitude * 10; // degrees*10e5 -> 10e7 latitude = _new_latitude;
longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude;
ground_speed_cm = _new_speed; ground_speed_cm = _new_speed;
ground_course_cd = _new_course; ground_course_cd = _new_course;
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
@ -248,8 +251,8 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
altitude_cm = _new_altitude; altitude_cm = _new_altitude;
time = _new_time; time = _new_time;
latitude = _new_latitude * 10; // degrees*10e5 -> 10e7 latitude = _new_latitude;
longitude = _new_longitude * 10; // degrees*10e5 -> 10e7 longitude = _new_longitude;
num_sats = _new_satellite_count; num_sats = _new_satellite_count;
hdop = _new_hdop; hdop = _new_hdop;
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix