diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 37788b968a..a15d925e0e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -62,6 +62,10 @@ bool AP_RangeFinder_NMEA::decode(char c) case '\n': case '*': { + if (_sentence_done) { + return false; + } + // null terminate and decode latest term _term[_term_offset] = 0; bool valid_sentence = decode_latest_term(); @@ -80,6 +84,7 @@ bool AP_RangeFinder_NMEA::decode(char c) _checksum = 0; _term_is_checksum = false; _distance_m = -1.0f; + _sentence_done = false; return false; } @@ -100,6 +105,7 @@ bool AP_RangeFinder_NMEA::decode_latest_term() { // handle the last term in a message if (_term_is_checksum) { + _sentence_done = true; uint8_t nibble_high = 0; uint8_t nibble_low = 0; if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 74b95eaa4e..81d1636fed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -58,8 +58,9 @@ private: char _term[15]; // buffer for the current term within the current sentence uint8_t _term_offset; // offset within the _term buffer where the next character should be placed uint8_t _term_number; // term index within the current sentence - float _distance_m = -1.0f; // distance in meters parsed from a term, -1 if no distance + float _distance_m = -1.0f; // distance in meters parsed from a term, -1 if no distance uint8_t _checksum; // checksum accumulator bool _term_is_checksum; // current term is the checksum sentence_types _sentence_type; // the sentence type currently being processed + bool _sentence_done; // true if this sentence has already been decoded };