diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 8f0ff4bc3f..4074916ff3 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -172,8 +172,13 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); } - const bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f) - && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f); + bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); + + // The test ratio does not need to pass all the time to have a valid estimate + if (!_valid) { + valid = valid && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f); + } + _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state();