Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed re-probing of NMEA GPS 

when we lost fix on a NMEA GPS we should retain the driver as long as
it is still sending us NMEA messages that pass the CRC check. This
fixes a bug where we would re-probe the GPS (searching baudrates) when
we lost position
This commit is contained in:
Andrew Tridgell 2022-11-22 18:29:19 +11:00
parent ad9dfe2d51
commit 02d6931c4d
2 changed files with 166 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -118,7 +118,6 @@ bool AP_GPS_NMEA::_decode(char c)
_crc32 = 0; _crc32 = 0;
_sentence_type = _GPS_SENTENCE_OTHER; _sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false; _is_checksum_term = false;
_gps_data_good = false;
_sentence_length = 1; _sentence_length = 1;
_sentence_done = false; _sentence_done = false;
return false; return false;
@ -293,7 +292,6 @@ bool AP_GPS_NMEA::_term_complete()
const uint32_t crc = strtoul(_term, nullptr, 16); const uint32_t crc = strtoul(_term, nullptr, 16);
const bool crc_ok = _is_unicore? (_crc32 == crc) : (_parity == crc); const bool crc_ok = _is_unicore? (_crc32 == crc) : (_parity == crc);
if (crc_ok) { if (crc_ok) {
if (_gps_data_good) {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_RMC: case _GPS_SENTENCE_RMC:
@ -470,19 +468,6 @@ bool AP_GPS_NMEA::_term_complete()
break; break;
} }
} }
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_RMC:
case _GPS_SENTENCE_GGA:
// Only these sentences give us information about
// fix status.
state.status = AP_GPS::NO_FIX;
break;
case _GPS_SENTENCE_THS:
state.have_gps_yaw = false;
break;
}
}
// see if we got a good message // see if we got a good message
return _have_new_message(); return _have_new_message();
} }
@ -501,12 +486,10 @@ bool AP_GPS_NMEA::_term_complete()
} }
if (strcmp(_term, "KSXT") == 0) { if (strcmp(_term, "KSXT") == 0) {
_sentence_type = _GPS_SENTENCE_KSXT; _sentence_type = _GPS_SENTENCE_KSXT;
_gps_data_good = true;
return false; return false;
} }
if (strcmp(_term, "AGRICA") == 0) { if (strcmp(_term, "AGRICA") == 0) {
_sentence_type = _GPS_SENTENCE_AGRICA; _sentence_type = _GPS_SENTENCE_AGRICA;
_gps_data_good = true;
return false; return false;
} }
/* /*
@ -526,15 +509,10 @@ bool AP_GPS_NMEA::_term_complete()
_sentence_type = _GPS_SENTENCE_GGA; _sentence_type = _GPS_SENTENCE_GGA;
} else if (strcmp(term_type, "HDT") == 0) { } else if (strcmp(term_type, "HDT") == 0) {
_sentence_type = _GPS_SENTENCE_HDT; _sentence_type = _GPS_SENTENCE_HDT;
// HDT doesn't have a data qualifier
_gps_data_good = true;
} else if (strcmp(term_type, "THS") == 0) { } else if (strcmp(term_type, "THS") == 0) {
_sentence_type = _GPS_SENTENCE_THS; _sentence_type = _GPS_SENTENCE_THS;
} else if (strcmp(term_type, "VTG") == 0) { } else if (strcmp(term_type, "VTG") == 0) {
_sentence_type = _GPS_SENTENCE_VTG; _sentence_type = _GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else { } else {
_sentence_type = _GPS_SENTENCE_OTHER; _sentence_type = _GPS_SENTENCE_OTHER;
} }
@ -547,17 +525,13 @@ bool AP_GPS_NMEA::_term_complete()
// operational status // operational status
// //
case _GPS_SENTENCE_RMC + 2: // validity (RMC) case _GPS_SENTENCE_RMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break; break;
case _GPS_SENTENCE_GGA + 6: // Fix data (GGA) case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0'; if (_term[0] > '0') {
_new_quality_indicator = _term[0] - '0'; _new_quality_indicator = _term[0] - '0';
break; } else {
case _GPS_SENTENCE_THS + 2: // validity (THS) _new_quality_indicator = 0;
_gps_data_good = _term[0] == 'A'; }
break;
case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break; break;
case _GPS_SENTENCE_GGA + 7: // satellite count (GGA) case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term); _new_satellite_count = atol(_term);
@ -622,10 +596,6 @@ bool AP_GPS_NMEA::_term_complete()
break; break;
case _GPS_SENTENCE_PHD + 2: // PHD message case _GPS_SENTENCE_PHD + 2: // PHD message
_phd.msg_id = atol(_term); _phd.msg_id = atol(_term);
if (_phd.msg_class == 1 && (_phd.msg_id == 12 || _phd.msg_id == 26)) {
// we only support $PHD messages 1/12 and 1/26
_gps_data_good = true;
}
break; break;
case _GPS_SENTENCE_PHD + 5: // PHD message, itow case _GPS_SENTENCE_PHD + 5: // PHD message, itow
_phd.itow = strtoul(_term, nullptr, 10); _phd.itow = strtoul(_term, nullptr, 10);

1
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -134,7 +134,6 @@ private:
uint16_t _term_number; ///< term index within the current sentence uint16_t _term_number; ///< term index within the current sentence
uint8_t _term_offset; ///< character offset with the term being received uint8_t _term_offset; ///< character offset with the term being received
uint16_t _sentence_length; uint16_t _sentence_length;
bool _gps_data_good; ///< set when the sentence indicates data is good
bool _sentence_done; ///< set when a sentence has been fully decoded bool _sentence_done; ///< set when a sentence has been fully decoded
// The result of parsing terms within a message is stored temporarily until // The result of parsing terms within a message is stored temporarily until