EKF: allow emergency reset in GNSS yaw and EV yaw aiding modes

Author: bresch

src/modules/ekf2/EKF/common.h

@@ -552,6 +552,7 @@ union warning_event_status_u {
 		bool stopping_mag_use			: 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
 		bool vision_data_stopped		: 1; ///< 9 - true when the vision system data has stopped for a significant time period
 		bool emergency_yaw_reset_mag_stopped	: 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+		bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -566,10 +566,11 @@ void Ekf::controlGpsFusion()
 
 		// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
 		// we can start using GPS
-		const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset
-						 && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
+		const bool align_yaw_using_gsf = !_control_status.flags.yaw_align
+		                                 && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE);
 
-		if (align_yaw_using_gsf) {
+		if ((align_yaw_using_gsf || _do_ekfgsf_yaw_reset)
+		    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
 			if (resetYawToEKFGSF()) {
 				_ekfgsf_yaw_reset_time = _time_last_imu;
 				_do_ekfgsf_yaw_reset = false;

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1680,16 +1680,32 @@ bool Ekf::resetYawToEKFGSF()
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 	_control_status.flags.yaw_align = true;
 
-	if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
+	const bool is_mag_fusion_active = _control_status.flags.mag_hdg
+	                                  || _control_status.flags.mag_3D;
+	const bool is_yaw_aiding_active = is_mag_fusion_active
+	                                  || _control_status.flags.gps_yaw
+					  || _control_status.flags.ev_yaw;
+
+	if (!is_yaw_aiding_active) {
 		_information_events.flags.yaw_aligned_to_imu_gps = true;
 		ECL_INFO("Yaw aligned using IMU and GPS");
 
 	} else {
-		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
-		// and cause another navigation failure
-		_control_status.flags.mag_fault = true;
-		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-		ECL_WARN("Emergency yaw reset - mag use stopped");
+		if (is_mag_fusion_active) {
+			// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
+			// and cause another navigation failure
+			_control_status.flags.mag_fault = true;
+			_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
+
+		} else if (_control_status.flags.gps_yaw) {
+			_is_gps_yaw_faulty = true;
+			_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
+
+		} else if (_control_status.flags.ev_yaw) {
+			_inhibit_ev_yaw_use = true;
+		}
+
+		ECL_WARN("Emergency yaw reset");
 	}
 
 	return true;

src/modules/ekf2/EKF/gps_checks.cpp

@@ -91,15 +91,9 @@ bool Ekf::collect_gps(const gps_message &gps)
 		_mag_strength_gps = get_mag_strength_gauss(lat, lon);
 
 		// request a reset of the yaw using the new declination
-		if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
-			// try to reset the yaw using the EKF-GSF yaw estimator
-			_do_ekfgsf_yaw_reset = true;
-			_ekfgsf_yaw_reset_time = 0;
-
-		} else {
-			if (!declination_was_valid) {
-				_mag_yaw_reset_req = true;
-			}
+		if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE)
+		     && !declination_was_valid) {
+			_mag_yaw_reset_req = true;
 		}
 
 		// save the horizontal and vertical position uncertainty of the origin