ekf-agp: add reset mode behavior

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -107,7 +107,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 					const bool fused = ekf.fuseHorizontalPosition(aid_src);
 					bool reset = false;
 
-					if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) {
+					if (!fused && isResetAllowed(ekf)) {
 						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 						ekf.resetAidSourceStatusZeroInnovation(aid_src);
 						reset = true;
@@ -139,7 +139,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
 				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
-					if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) {
+					if (isResetAllowed(ekf)) {
 
 						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 
@@ -178,4 +178,12 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 	}
 }
 
+bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
+{
+	return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
+		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
+	       || ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
+		   && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
+}
+
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

@@ -82,6 +82,8 @@ private:
 		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
 	}
 
+	bool isResetAllowed(const Ekf &ekf) const;
+
 	struct AuxGlobalPositionSample {
 		uint64_t time_us{};     ///< timestamp of the measurement (uSec)
 		double latitude{};
@@ -101,6 +103,11 @@ private:
 		VPOS  = (1 << 1)
 	};
 
+	enum class Mode : uint8_t {
+		kAuto           = 0,   	///< Reset on fusion timeout if no other source of position is available
+		kDeadReckoning = 1   	///< Reset on fusion timeout if no source of velocity is availabl
+	};
+
 	enum class State {
 		stopped,
 		starting,
@@ -122,6 +129,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::EKF2_AGP_CTRL>) _param_ekf2_agp_ctrl,
+		(ParamInt<px4::params::EKF2_AGP_MODE>) _param_ekf2_agp_mode,
 		(ParamFloat<px4::params::EKF2_AGP_DELAY>) _param_ekf2_agp_delay,
 		(ParamFloat<px4::params::EKF2_AGP_NOISE>) _param_ekf2_agp_noise,
 		(ParamFloat<px4::params::EKF2_AGP_GATE>) _param_ekf2_agp_gate

src/modules/ekf2/params_aux_global_position.yaml

@@ -14,6 +14,16 @@ parameters:
       default: 0
       min: 0
       max: 3
+    EKF2_AGP_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_AGP_DELAY:
       description:
         short: Aux global position estimator delay relative to IMU measurements