Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: better handle corrupted NMEA sentences

Browse Source 
when a NMEA sentence is corrupted we should discard it
completely. This change prevents us considering a set of sentences
that includes corrupted sentences from being seen as new data
This commit is contained in:
Andrew Tridgell 2016-02-10 09:39:32 +11:00
parent cb60384dc4
commit 4beb1748c5
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -288,8 +288,10 @@ bool AP_GPS_NMEA::_term_complete()
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]); uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) { if (checksum == _parity) {
if (_gps_data_good) { if (_gps_data_good) {
uint32_t now = AP_HAL::millis();
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_GPRMC:
_last_GPRMC_ms = now;
//time = _new_time; //time = _new_time;
//date = _new_date; //date = _new_date;
state.location.lat = _new_latitude; state.location.lat = _new_latitude;
@ -297,12 +299,13 @@ bool AP_GPS_NMEA::_term_complete()
state.ground_speed = _new_speed*0.01f; state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_new_course); state.ground_course_cd = wrap_360_cd(_new_course);
make_gps_time(_new_date, _new_time * 10); make_gps_time(_new_date, _new_time * 10);
state.last_gps_time_ms = AP_HAL::millis(); state.last_gps_time_ms = now;
// To-Do: add support for proper reporting of 2D and 3D fix // To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D; state.status = AP_GPS::GPS_OK_FIX_3D;
fill_3d_velocity(); fill_3d_velocity();
break; break;
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
_last_GPGGA_ms = now;
state.location.alt = _new_altitude; state.location.alt = _new_altitude;
state.location.lat = _new_latitude; state.location.lat = _new_latitude;
state.location.lng = _new_longitude; state.location.lng = _new_longitude;
@ -312,6 +315,7 @@ bool AP_GPS_NMEA::_term_complete()
state.status = AP_GPS::GPS_OK_FIX_3D; state.status = AP_GPS::GPS_OK_FIX_3D;
break; break;
case _GPS_SENTENCE_GPVTG: case _GPS_SENTENCE_GPVTG:
_last_GPVTG_ms = now;
state.ground_speed = _new_speed*0.01f; state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = wrap_360_cd(_new_course); state.ground_course_cd = wrap_360_cd(_new_course);
// VTG has no fix indicator, can't change fix status // VTG has no fix indicator, can't change fix status
@ -337,15 +341,12 @@ bool AP_GPS_NMEA::_term_complete()
if (_term_number == 0) { if (_term_number == 0) {
if (!strcmp(_term, _gprmc_string)) { if (!strcmp(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC; _sentence_type = _GPS_SENTENCE_GPRMC;
_last_GPRMC_ms = AP_HAL::millis();
} else if (!strcmp(_term, _gpgga_string)) { } else if (!strcmp(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA; _sentence_type = _GPS_SENTENCE_GPGGA;
_last_GPGGA_ms = AP_HAL::millis();
} else if (!strcmp(_term, _gpvtg_string)) { } else if (!strcmp(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG; _sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good // VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise. // unless it tells us otherwise.
_last_GPVTG_ms = AP_HAL::millis();
_gps_data_good = true; _gps_data_good = true;
} else { } else {
_sentence_type = _GPS_SENTENCE_OTHER; _sentence_type = _GPS_SENTENCE_OTHER;