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 3307dd56b9..675bffdd3c 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 @@ -104,9 +104,20 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed _state = State::starting; if (ekf.global_origin_valid()) { - ekf.enableControlStatusAuxGpos(); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - _state = State::active; + const bool fused = ekf.fuseHorizontalPosition(aid_src); + bool reset = false; + + if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) { + ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _state = State::active; + } } else { // Try to initialize using measurement @@ -128,12 +139,18 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) { - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - ekf.resetAidSourceStatusZeroInnovation(aid_src); + ekf.resetAidSourceStatusZeroInnovation(aid_src); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } } } else { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 84ced94c80..22ccf958f0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -600,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f } int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources(); +} + +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { return int(_control_status.flags.gps) - + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + + int(_control_status.flags.aux_gpos); +} + +int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const +{ + return int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_vel) - + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1212300278..93632bc109 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -207,8 +207,8 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -221,6 +221,7 @@ public: * @return true if an other source than aiding_flag is active */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalPositionAidingThan(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 @@ -228,6 +229,8 @@ public: bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; + int getNumberOfActiveHorizontalPositionAidingSources() const; + int getNumberOfActiveHorizontalVelocityAidingSources() const; bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; bool isVerticalPositionAidingActive() const;