AP_GPS: fixed handling of empty HDT/THS sentences

fixes ArduPilot#22432
tridge · Dec 19, 2022 · e4e784a · e4e784a
1 parent a43709c
commit e4e784a
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -48,6 +48,9 @@ extern const AP_HAL::HAL& hal;
 #define AP_GPS_NMEA_CONFIG_PERIOD_MS 15000U
 #endif
 
+// a quiet nan for invalid values
+#define QNAN nanf("GPS")
+
 // Convenience macros //////////////////////////////////////////////////////////
 //
 #define DIGIT_TO_VAL(_x)        (_x - '0')
@@ -120,6 +123,7 @@ bool AP_GPS_NMEA::_decode(char c)
         _is_checksum_term = false;
         _sentence_length = 1;
         _sentence_done = false;
+        _new_gps_yaw = QNAN;
         return false;
     }
 
@@ -377,6 +381,10 @@ bool AP_GPS_NMEA::_term_complete()
                     // use AGRICA
                     break;
                 }
+                if (isnan(_new_gps_yaw)) {
+                    // empty sentence
+                    break;
+                }
                 _last_yaw_ms = now;
                 state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
                 state.have_gps_yaw = true;