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

AP_GPS: prevent extra NMEA messages causing rapid fix rates

Browse Source 
unexpected messages could cause the GPS driver to report a very high
fix rate, which can confuse the EKF
This commit is contained in:
Andrew Tridgell 2015-07-03 22:08:05 +10:00
parent b20318aaeb
commit 21c895f6f3
2 changed files with 39 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -235,6 +235,33 @@ uint32_t AP_GPS_NMEA::_parse_degrees()
return ret;
}
/*
see if we have a new set of NMEA messages
*/
bool AP_GPS_NMEA::_have_new_message()
{
if (_last_GPRMC_ms == 0 ||
_last_GPGGA_ms == 0) {
return false;
}
uint32_t now = hal.scheduler->millis();
if (now - _last_GPRMC_ms > 150 ||
now - _last_GPGGA_ms > 150) {
return false;
}
if (_last_GPVTG_ms != 0 &&
now - _last_GPVTG_ms > 150) {
return false;
}
// prevent these messages being used again
if (_last_GPVTG_ms != 0) {
_last_GPVTG_ms = 1;
}
_last_GPGGA_ms = 1;
_last_GPRMC_ms = 1;
return true;
}
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
@ -257,6 +284,7 @@ bool AP_GPS_NMEA::_term_complete()
// To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D;
fill_3d_velocity();
_last_GPRMC_ms = state.last_gps_time_ms;
break;
case _GPS_SENTENCE_GPGGA:
state.location.alt = _new_altitude;
@ -266,10 +294,12 @@ bool AP_GPS_NMEA::_term_complete()
state.hdop = _new_hdop;
// To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D;
_last_GPGGA_ms = hal.scheduler->millis();
break;
case _GPS_SENTENCE_GPVTG:
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = _new_course;
_last_GPVTG_ms = hal.scheduler->millis();
// VTG has no fix indicator, can't change fix status
break;
}
@ -282,8 +312,8 @@ bool AP_GPS_NMEA::_term_complete()
state.status = AP_GPS::NO_FIX;
}
}
// we got a good message
return true;
// see if we got a good message
return _have_new_message();
}
// we got a bad message, ignore it
return false;

7
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -115,6 +115,9 @@ private:
/// an update to the GPS state.
bool _term_complete();
/// return true if we have a new set of NMEA messages
bool _have_new_message(void);
uint8_t _parity; ///< NMEA message checksum accumulator
bool _is_checksum_term; ///< current term is the checksum
char _term[15]; ///< buffer for the current term within the current sentence
@ -136,6 +139,10 @@ private:
uint16_t _new_hdop; ///< HDOP parsed from a term
uint8_t _new_satellite_count; ///< satellite count parsed from a term
uint32_t _last_GPRMC_ms = 0;
uint32_t _last_GPGGA_ms = 0;
uint32_t _last_GPVTG_ms = 0;
/// @name Init strings
/// In ::init, an attempt is made to configure the GPS
/// unit to send just the messages that we are interested