ekf2-gnss: add reset mode

This allows the user to choose whether the position should immediately
be reset to GNSS on fusion timeout or if the EKF can continue with
velocity dead-reckoning.

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -104,10 +104,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 		bool do_vel_pos_reset = false;
 
-		if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
-			// Reset checks need to run together, before each control function runs because the result would change
-			// after the first one runs
-			do_vel_pos_reset = shouldResetGpsFusion();
+		if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) {
 
 			if (_control_status.flags.in_air
 			    && isYawFailure()
@@ -131,16 +128,17 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 
 	if (_control_status.flags.gnss_vel) {
 		if (continuing_conditions_passing) {
-			if (force_reset) {
-				ECL_WARN("GNSS fusion timeout, resetting");
-				resetVelocityToGnss(aid_src);
+			fuseVelocity(aid_src);
 
-			} else {
-				fuseVelocity(aid_src);
+			const bool fusion_timeout = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
 
-				if (isHeightResetRequired()) {
-					// reset vertical velocity if height is failing
-					resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
+			if (fusion_timeout || force_reset) {
+				if (isGnssVelResetAllowed() || force_reset) {
+					ECL_WARN("GNSS fusion timeout, resetting");
+					resetVelocityToGnss(aid_src);
+
+				} else {
+					stopGnssVelFusion();
 				}
 			}
 
@@ -150,19 +148,27 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 
 	} else {
 		if (starting_conditions_passing) {
-			ECL_INFO("starting GNSS velocity fusion");
-			_information_events.flags.starting_gps_fusion = true;
+			bool fused = false;
 
-			// when already using another velocity source velocity reset is not necessary
-			if (!isHorizontalAidingActive()
-			    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-			    || !_control_status_prev.flags.yaw_align
-			   ) {
-				// reset velocity
-				resetVelocityToGnss(aid_src);
+			const bool do_reset = force_reset || !_control_status_prev.flags.yaw_align;
+
+			// Start fusing the data without reset if possible to avoid disturbing the filter
+			if (!do_reset && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) {
+				fused = fuseVelocity(aid_src);
 			}
 
-			_control_status.flags.gnss_vel = true;
+			bool reset = false;
+
+			if (!fused && (isGnssVelResetAllowed() || force_reset)) {
+				resetVelocityToGnss(aid_src);
+				reset = true;
+			}
+
+			if (fused || reset) {
+				ECL_INFO("starting GNSS velocity fusion");
+				_information_events.flags.starting_gps_fusion = true;
+				_control_status.flags.gnss_vel = true;
+			}
 		}
 	}
 }
@@ -179,12 +185,19 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for
 
 	if (_control_status.flags.gnss_pos) {
 		if (continuing_conditions_passing) {
-			if (force_reset) {
-				ECL_WARN("GNSS fusion timeout, resetting");
-				resetHorizontalPositionToGnss(aid_src);
+			fuseHorizontalPosition(aid_src);
 
-			} else {
-				fuseHorizontalPosition(aid_src);
+			const bool fusion_timeout = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+
+			if (fusion_timeout || force_reset) {
+				if (isGnssPosResetAllowed()) {
+					ECL_WARN("GNSS fusion timeout, resetting");
+					resetHorizontalPositionToGnss(aid_src);
+
+				} else {
+					stopGnssPosFusion();
+					_control_status.flags.gnss_fault = true;
+				}
 			}
 
 		} else {
@@ -193,11 +206,30 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for
 
 	} else {
 		if (starting_conditions_passing) {
-			ECL_INFO("starting GNSS position fusion");
-			_information_events.flags.starting_gps_fusion = true;
+			bool fused = false;
 
-			resetHorizontalPositionToGnss(aid_src);
-			_control_status.flags.gnss_pos = true;
+			const bool do_reset = force_reset || !_control_status_prev.flags.yaw_align;
+
+			// Start fusing the data without reset if possible to avoid disturbing the filter
+			if (_local_origin_lat_lon.isInitialized()
+			    && !do_reset
+			    && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) {
+				fused = fuseHorizontalPosition(aid_src);
+			}
+
+			bool reset = false;
+
+			if (!fused && isGnssPosResetAllowed()) {
+				resetHorizontalPositionToGnss(aid_src);
+				reset = true;
+			}
+
+			if (fused || reset) {
+				ECL_INFO("starting GNSS position fusion");
+				_information_events.flags.starting_gps_fusion = true;
+				_control_status.flags.gnss_pos = true;
+				_control_status.flags.gnss_fault = false;
+			}
 
 		} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
 			resetHorizontalPositionToGnss(aid_src);
@@ -205,6 +237,61 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for
 	}
 }
 
+bool Ekf::isGnssVelResetAllowed() const
+{
+	if (_control_status.flags.gnss_fault) {
+		return false;
+	}
+
+	bool allowed = true;
+
+	switch (static_cast<GnssMode>(_params.ekf2_gps_mode)) {
+	case GnssMode::AUTO:
+		if (isOtherSourceOfHorizontalVelocityAidingThan(_control_status.flags.gnss_vel)
+		    && !_control_status.flags.wind_dead_reckoning) {
+			allowed = false;
+		}
+
+		break;
+
+	case GnssMode::DEAD_RECKONING:
+		if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_vel)) {
+			allowed = false;
+		}
+
+		break;
+	}
+
+	return allowed;
+}
+
+bool Ekf::isGnssPosResetAllowed() const
+{
+	if (_control_status.flags.gnss_fault) {
+		return false;
+	}
+
+	bool allowed = true;
+
+	switch (static_cast<GnssMode>(_params.ekf2_gps_mode)) {
+	case GnssMode::AUTO:
+		if (isOtherSourceOfHorizontalPositionAidingThan(_control_status.flags.gnss_pos)) {
+			allowed = false;
+		}
+
+		break;
+
+	case GnssMode::DEAD_RECKONING:
+		if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
+			allowed = false;
+		}
+
+		break;
+	}
+
+	return allowed;
+}
+
 void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
 {
 	// correct velocity for offset relative to IMU
@@ -358,38 +445,6 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
 	resetAidSourceStatusZeroInnovation(aid_src);
 }
 
-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
-	 */
-	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);
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-
-	if (has_horizontal_aiding_timed_out) {
-		// horizontal aiding hasn't timed out if optical flow still active
-		if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
-			has_horizontal_aiding_timed_out = false;
-		}
-	}
-
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-	const bool is_reset_required = has_horizontal_aiding_timed_out
-				       || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
-					   && (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
-
-	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_inflight_nav_failure);
-}
-
 void Ekf::stopGnssFusion()
 {
 	if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {

src/modules/ekf2/EKF/common.h

@@ -148,6 +148,11 @@ enum class GnssCtrl : uint8_t {
 	YAW  = (1 << 3)
 };
 
+enum class GnssMode : uint8_t {
+	AUTO           = 0,   	///< Reset on fusion timeout if no other source of position is available
+	DEAD_RECKONING = 1   	///< Reset on fusion timeout if no source of velocity is availabl
+};
+
 enum class RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
@@ -322,6 +327,7 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_GNSS)
 	int32_t ekf2_gps_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
+	int32_t ekf2_gps_mode {static_cast<int32_t>(GnssMode::AUTO)};
 	float ekf2_gps_delay{110.0f};           ///< GPS measurement delay relative to the IMU (mSec)
 
 	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
@@ -599,6 +605,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 		uint64_t baro_fault              : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
+uint64_t gnss_fault              :
+		1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/ekf.h

@@ -861,11 +861,12 @@ private:
 	void stopGnssPosFusion();
 	void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
 	void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
+	bool isGnssVelResetAllowed() const;
+	bool isGnssPosResetAllowed() const;
 	void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
 	bool tryYawEmergencyReset();
 	void resetVelocityToGnss(estimator_aid_source3d_s &aid_src);
 	void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src);
-	bool shouldResetGpsFusion() const;
 
 	void controlGnssHeightFusion(const gnssSample &gps_sample);
 	void stopGpsHgtFusion();

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -627,6 +627,17 @@ bool EstimatorInterface::isHorizontalPositionAidingActive() const
 	return getNumberOfActiveHorizontalPositionAidingSources() > 0;
 }
 
+bool EstimatorInterface::isOnlyActiveSourceOfHorizontalVelocityAiding(const bool aiding_flag) const
+{
+	return aiding_flag && !isOtherSourceOfHorizontalVelocityAidingThan(aiding_flag);
+}
+
+bool EstimatorInterface::isOtherSourceOfHorizontalVelocityAidingThan(const bool aiding_flag) const
+{
+	const int nb_sources = getNumberOfActiveHorizontalVelocityAidingSources();
+	return aiding_flag ? nb_sources > 1 : nb_sources > 0;
+}
+
 int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const
 {
 	return int(_control_status.flags.gnss_vel)

src/modules/ekf2/EKF/estimator_interface.h

@@ -214,6 +214,7 @@ public:
 
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
 	bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const;
+	bool isOnlyActiveSourceOfHorizontalVelocityAiding(bool aiding_flag) const;
 
 	/*
 	 * Check if there are any other active source of horizontal aiding
@@ -227,6 +228,7 @@ public:
 	 */
 	bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
 	bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const;
+	bool isOtherSourceOfHorizontalVelocityAidingThan(bool aiding_flag) const;
 
 	// Return true if at least one source of horizontal aiding is active
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos

src/modules/ekf2/EKF2.cpp

@@ -80,6 +80,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_noaid_noise(_params->ekf2_noaid_noise),
 #if defined(CONFIG_EKF2_GNSS)
 	_param_ekf2_gps_ctrl(_params->ekf2_gps_ctrl),
+	_param_ekf2_gps_mode(_params->ekf2_gps_mode),
 	_param_ekf2_gps_delay(_params->ekf2_gps_delay),
 	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
 	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),

src/modules/ekf2/EKF2.hpp

@@ -513,6 +513,7 @@ private:
 
 #if defined(CONFIG_EKF2_GNSS)
 		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
+		(ParamExtInt<px4::params::EKF2_GPS_MODE>) _param_ekf2_gps_mode,
 		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
 
 		(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,

src/modules/ekf2/params_gnss.yaml

@@ -17,6 +17,16 @@ parameters:
       default: 7
       min: 0
       max: 15
+    EKF2_GPS_MODE:
+      description:
+        short: Fusion reset mode
+        long: 'Automatic: reset on fusion timeout if no other source of position is available
+          Dead-reckoning: reset on fusion timeout if no source of velocity is available'
+      type: enum
+      values:
+        0: Automatic
+        1: Dead-reckoning
+      default: 0
     EKF2_GPS_DELAY:
       description:
         short: GPS measurement delay relative to IMU measurements