diff --git a/EKF/control.cpp b/EKF/control.cpp
index 9ebd9976e0..e9e6aa5ff4 100644
--- a/EKF/control.cpp
+++ b/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
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 6e5108ab61..a2f73053c7 100644
--- a/EKF/ekf.h
+++ b/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)
diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp
index 1d56c799c3..b40dad9773 100644
--- a/test/sensor_simulator/ekf_wrapper.cpp
+++ b/test/sensor_simulator/ekf_wrapper.cpp
@@ -170,6 +170,20 @@ void EkfWrapper::disableExternalVisionAlignment()
 	_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
 }
 
+bool EkfWrapper::isIntendingMagHeadingFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.mag_hdg;
+}
+
+bool EkfWrapper::isIntendingMag3DFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.mag_3D;
+}
+
 bool EkfWrapper::isWindVelocityEstimated() const
 {
 	filter_control_status_u control_status;
diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h
index ca3128f1ac..1808d00cea 100644
--- a/test/sensor_simulator/ekf_wrapper.h
+++ b/test/sensor_simulator/ekf_wrapper.h
@@ -84,6 +84,9 @@ public:
 	void disableExternalVisionHeadingFusion();
 	bool isIntendingExternalVisionHeadingFusion() const;
 
+	bool isIntendingMagHeadingFusion() const;
+	bool isIntendingMag3DFusion() const;
+
 	void enableExternalVisionAlignment();
 	void disableExternalVisionAlignment();
 
diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp
index b44073e03d..50629470e5 100644
--- a/test/test_EKF_gps_yaw.cpp
+++ b/test/test_EKF_gps_yaw.cpp
@@ -117,3 +117,30 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f))
 		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
 }
+
+TEST_F(EkfGpsHeadingTest, fallBackToMag)
+{
+	// GIVEN: an initial GPS yaw, not aligned with the current one
+	float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
+	_sensor_simulator._gps.setYaw(gps_heading);
+
+	// WHEN: the GPS yaw fusion is activated
+	_ekf_wrapper.enableGpsHeadingFusion();
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: GPS heading fusion should have started, and mag
+	// fusion should be disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
+	// BUT WHEN: the GPS yaw is suddenly invalid
+	gps_heading = NAN;
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(6);
+
+	// THEN: after a few seconds, the fusion should stop and
+	// the estimator should fall back to mag fusion
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+}