EKF: Allow reset of yaw to EKF-GSF later in flight

EKF/common.h

@@ -367,8 +367,9 @@ struct parameters {
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
 	float EKFGSF_tas_default{15.0f};	///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
-	unsigned EKFGSF_reset_delay{1000000};	///< Number of uSec of bad innovations on main filter inpost takeoff phase before yaw is reset to EKF-GSF value
+	unsigned EKFGSF_reset_delay{1000000};	///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	float EKFGSF_yaw_err_max{0.262f}; 	///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
+	unsigned EKFGSF_reset_count_limit{3};	///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
 };
 
 struct stateSample {

EKF/control.cpp

@@ -631,21 +631,42 @@ void Ekf::controlGpsFusion()
 			// We haven't had an absolute position fix for a longer time so need to do something
 			do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
-			// A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable
+			/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
-			// recovery from a bad magnetometer or field estimate.
+			   navigation casued by a bad yaw estimate.
-			// This special case reset can also be requested externally.
+
-			// The minimum time interval between resets to the EKF-GSF estimate must be limited to
+			   A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
-			// allow the EKF-GSF time to improve its estimate if the first reset was not successful.
+			   innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
-			// Don't perform the reset if getting GPS back after a significant period of no data because the timeout
+			   different test criteria are used that take longer to trigger and reduce false positives. A reset is
-			// could have been caused by bad GPS
+			   not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
-			const bool stopped_following_gps_velocity = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
+			   or other sensor errors.
-							   (_time_last_hor_vel_fuse > _time_last_on_ground_us);
+
+			   The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
+
+			   The total number of resets allowed per boot cycle is limited.
+
+			   The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
+			   to improve its estimate if the previous reset was not successful.
+
+			   A reset is not performed when getting GPS back after a significant period of no data because the timeout
+			   could have been caused by bad GPS.
+			*/
+
 			if (!_control_status.flags.in_air) {
 				_time_last_on_ground_us = _time_last_imu;
 			}
-			const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000);
+
-			const bool do_yaw_vel_pos_reset = (do_vel_pos_reset || _do_ekfgsf_yaw_reset || stopped_following_gps_velocity) &&
+			const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&
-								recent_takeoff &&
+								!isTimedOut(_time_last_on_ground_us, 30000000) &&
+								isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
+								(_time_last_hor_vel_fuse > _time_last_on_ground_us);
+
+			const bool inflight_nav_failure = _control_status.flags.in_air &&
+							  do_vel_pos_reset &&
+							  (_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
+							  (_time_last_hor_pos_fuse > _time_last_on_ground_us);
+
+			const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || recent_takeoff_nav_failure || inflight_nav_failure) &&
+							  _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit &&
 							  isTimedOut(_ekfgsf_yaw_reset_time, 5000000) &&
 							  !gps_signal_was_lost;
 
@@ -653,6 +674,7 @@ void Ekf::controlGpsFusion()
 				if (resetYawToEKFGSF()) {
 					_ekfgsf_yaw_reset_time = _time_last_imu;
 					_do_ekfgsf_yaw_reset = false;
+					_ekfgsf_yaw_reset_count++;
 
 					// Reset the timeout counters
 					_time_last_hor_pos_fuse = _time_last_imu;

EKF/ekf.h

@@ -876,6 +876,7 @@ private:
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	uint64_t _time_last_on_ground_us{0};	///< last tine we were on the ground (uSec)
 	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested
+	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
 
 	// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
 	void runYawEKFGSF();