diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 8e617bdb63..b96f9c63c5 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -335,9 +335,11 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl const bool innov_rejected = (test_ratio > 1.f); - if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) { - // when on ground or accuracy chosen to be very low, we hard reset position + if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected || isHorizontalPositionAidingActive()) { + // When on ground or accuracy chosen to be very low, we hard reset position // this allows the user to still send hard resets at any time + // Also reset when another position source is active as it it would otherwise have almost no + // visible effect to the position estimate. ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 9e0ebec771..7acdc5505b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -622,6 +622,11 @@ int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const + int(_control_status.flags.aux_gpos); } +bool EstimatorInterface::isHorizontalPositionAidingActive() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() > 0; +} + int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const { return int(_control_status.flags.gnss_vel) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 56e8334fba..43e6dca48c 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -231,6 +231,7 @@ public: // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isHorizontalAidingActive() const; + bool isHorizontalPositionAidingActive() const; bool isVerticalAidingActive() const; bool isNorthEastAidingActive() const; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 785f5565f4..f70eca2b39 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -531,10 +531,9 @@ void EKF2::Run() } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { - if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning - || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gnss_pos)) - && PX4_ISFINITE(vehicle_command.param2) - && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) + if (PX4_ISFINITE(vehicle_command.param2) + && PX4_ISFINITE(vehicle_command.param5) + && PX4_ISFINITE(vehicle_command.param6) ) { const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,