ekf2: Only reset to GNSS heading if necessary

When North-East (e.g.: GNSS pos/vel) aiding is active, the heading
estimate is constrained and consistent with the vel/pos aiding. Reset to
GNSS heading should only occur if no N-E aiding is active or if the
filter is not yes aligned. Otherwise, just wait for the consistency
check to pass again (will pass at some point if the heading uncertainty
of the filter is getting too high).

src/modules/ekf2/EKF/ekf.h

@@ -672,7 +672,6 @@ private:
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	estimator_aid_source1d_s _aid_src_gnss_yaw{};
-	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -358,7 +358,6 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 				&& !_gps_intermittent;
 
 		if (_control_status.flags.gps_yaw) {
-
 			if (continuing_conditions_passing) {
 
 				fuseGpsYaw(gps_sample.yaw_offset);
@@ -366,27 +365,7 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
-					if (_nb_gps_yaw_reset_available > 0) {
-						// Data seems good, attempt a reset
-						resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);
-
-						if (_control_status.flags.in_air) {
-							_nb_gps_yaw_reset_available--;
-						}
-
-					} else if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						_control_status.flags.gps_yaw_fault = true;
-						stopGpsYawFusion();
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						stopGpsYawFusion();
-					}
-
-					// TODO: should we give a new reset credit when the fusion does not fail for some time?
+					stopGpsYawFusion();
 				}
 
 			} else {
@@ -397,14 +376,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
-				if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-					ECL_INFO("starting GPS yaw fusion");
+				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
 
-					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
+				if (!_control_status.flags.in_air
+				    || !_control_status.flags.yaw_align
+				    || not_using_ne_aiding) {
+
+					// Reset before starting the fusion
+					if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
+						_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
+						_control_status.flags.gps_yaw = true;
+						_control_status.flags.yaw_align = true;
+					}
+
+				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
+					// Do not force a reset but wait for the consistency check to pass
 					_control_status.flags.gps_yaw = true;
-					_control_status.flags.yaw_align = true;
+					fuseGpsYaw(gps_sample.yaw_offset);
+				}
 
-					_nb_gps_yaw_reset_available = 1;
+				if (_control_status.flags.gps_yaw) {
+					ECL_INFO("starting GPS yaw fusion");
 				}
 			}
 		}

src/modules/ekf2/test/test_EKF_gps_yaw.cpp

@@ -275,9 +275,15 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(8);
 
-	// THEN: the fusion should reset
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	// THEN: the fusion should stop, reset to mag
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+
+	// AND THEN: restart GNSS yaw fusion
+	_sensor_simulator.runSeconds(5);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
 	EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
 }
 
@@ -285,7 +291,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 {
 	// GIVEN: the GPS yaw fusion activated
 	float gps_heading = _ekf_wrapper.getYawAngle();
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f));
 	_sensor_simulator.runSeconds(5);
 	_ekf->set_in_air_status(true);
 
@@ -295,20 +301,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(7.5);
 
-	// THEN: the fusion should reset
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
-
-	// BUT WHEN: the measurement jumps a 2nd time
-	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
-	_sensor_simulator._gps.setYaw(gps_heading);
-	_sensor_simulator.runSeconds(7.5);
+	// THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
 
 	// THEN: after a few seconds, the fusion should stop and
 	// the estimator doesn't fall back to mag fusion because it has
 	// been declared inconsistent with the filter states
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent());
-	//TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop?
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 }