ekf2: use yaw emergency estimator more aggressively

During the whole flight, if the difference between the yaw estimate from
EKF2 and the emergency estimator is large and that the GNSS velocity
fusion is failing continuously, immediately reset to the emergency yaw
estimate.

src/modules/ekf2/EKF/common.h

@@ -408,11 +408,11 @@ struct parameters {
 	float acc_bias_learn_gyr_lim{3.0f};     ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
 	float acc_bias_learn_tc{0.5f};          ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	const unsigned reset_timeout_max{7000000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
-	const unsigned no_aid_timeout_max{1000000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
+	const unsigned reset_timeout_max{7'000'000};      ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
+	const unsigned no_aid_timeout_max{1'000'000};     ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
 	const unsigned hgt_fusion_timeout_max{5'000'000}; ///< maximum time we allow height fusion to fail before attempting a reset or stopping the fusion aiding (uSec)
 
-	int32_t valid_timeout_max{5000000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
@@ -447,7 +447,6 @@ struct parameters {
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const 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
 	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
-	const unsigned EKFGSF_reset_count_limit{3};     ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
 };
 
 union fault_status_u {

src/modules/ekf2/EKF/control.cpp

@@ -612,10 +612,3 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
-
-bool Ekf::hasHorizontalAidingTimedOut() const
-{
-	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
-}

src/modules/ekf2/EKF/ekf.h

@@ -875,7 +875,6 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
-	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
 
 	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
@@ -1060,9 +1059,6 @@ private:
 	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 
-	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
-	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
-
 	void runYawEKFGSF();
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1434,9 +1434,6 @@ bool Ekf::resetYawToEKFGSF()
 		_inhibit_ev_yaw_use = true;
 	}
 
-	_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
-	_ekfgsf_yaw_reset_count++;
-
 	return true;
 }
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -112,25 +112,24 @@ void Ekf::controlGpsFusion()
 					fuseVelocity(_aid_src_gnss_vel);
 					fuseHorizontalPosition(_aid_src_gnss_pos);
 
-					if (shouldResetGpsFusion()) {
-						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
+					bool do_vel_pos_reset = shouldResetGpsFusion();
 
-						/* 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.
-						 * The total number of resets allowed per boot cycle is limited.
+					if (isYawFailure()
+					    && _control_status.flags.in_air
+					    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+					    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+						/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
+						 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
+						 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
+						 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
 						 */
-						if (isYawFailure()
-						    && _control_status.flags.in_air
-						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
-						    && isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
-							// 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.
-							if (resetYawToEKFGSF()) {
-								ECL_WARN("GPS emergency yaw reset");
-							}
+						if (resetYawToEKFGSF()) {
+							ECL_WARN("GPS emergency yaw reset");
+							do_vel_pos_reset = true;
 						}
+					}
 
+					if (do_vel_pos_reset) {
 						ECL_WARN("GPS fusion timeout, resetting velocity and position");
 						_information_events.flags.reset_vel_to_gps = true;
 						_information_events.flags.reset_pos_to_gps = true;
@@ -221,30 +220,20 @@ bool Ekf::shouldResetGpsFusion() const
 	/* We are relying on aiding to constrain drift so after a specified time
 	 * with no aiding we need to do something
 	 */
-	const bool is_reset_required = hasHorizontalAidingTimedOut()
+	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
+
+	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
-	/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
-	 * navigation casued by a bad yaw estimate.
-
-	 * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
-	 * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
-	 * different test criteria are used that take longer to trigger and reduce false positives. A reset is
-	 * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
-	 * or other sensor errors.
-	 */
-	const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air
-			&& isRecent(_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 is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 					     && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
 					     && (_time_last_hor_vel_fuse > _time_last_on_ground_us)
 					     && (_time_last_hor_pos_fuse > _time_last_on_ground_us);
 
-	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
+	return (is_reset_required || is_inflight_nav_failure);
 }
 
 bool Ekf::isYawFailure() const

src/modules/ekf2/EKF/mag_control.cpp

@@ -209,7 +209,6 @@ void Ekf::runInAirYawReset()
 			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
 			_information_events.flags.yaw_aligned_to_imu_gps = true;
-			_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
 
 			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
 			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {