diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index bae4849c39..6c74c323d1 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/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(_param_ekf2_agp_mode.get()) == Mode::kAuto) + && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) + || ((static_cast(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning) + && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos)); +} + #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 423462d6ee..3834f0fcb4 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/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) _param_ekf2_agp_ctrl, + (ParamInt) _param_ekf2_agp_mode, (ParamFloat) _param_ekf2_agp_delay, (ParamFloat) _param_ekf2_agp_noise, (ParamFloat) _param_ekf2_agp_gate diff --git a/src/modules/ekf2/params_aux_global_position.yaml b/src/modules/ekf2/params_aux_global_position.yaml index 77f348776b..93c7165226 100644 --- a/src/modules/ekf2/params_aux_global_position.yaml +++ b/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