GPS Yaw: fall back to other yaw aiding source in case of bad data

If the user selects GPS yaw fusion but that there is no GPS yaw data in
the GPS message or if the fusion is rejected for some time, the GPS yaw
data is declared faulty and the fusion is stopped to allow an other
source of yaw aiding to start.

EKF/control.cpp

@@ -518,7 +518,10 @@ void Ekf::controlGpsFusion()
 		// Detect if coming back after significant time without GPS data
 		bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
-		if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
+
+		if (!(_params.fusion_mode & MASK_USE_GPSYAW)
+		    || _is_gps_yaw_faulty) {
+
 			stopGpsYawFusion();
 
 		} else {
@@ -541,6 +544,14 @@ void Ekf::controlGpsFusion()
 					fuseGpsAntYaw();
 				}
 			}
+
+			// Check if the data is constantly fused by the estimator,
+			// if not, declare the sensor faulty and stop the fusion
+			// By doing this, another source of yaw aiding is allowed to start
+			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
+				_is_gps_yaw_faulty = true;
+				stopGpsYawFusion();
+			}
 		}
 
 		// Determine if we should use GPS aiding for velocity and horizontal position

EKF/ekf.h

@@ -499,6 +499,7 @@ private:
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
 	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
+	bool _is_gps_yaw_faulty{false};		///< true if gps yaw data is rejected by the filter for too long
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)