From 9cedb169fcb3474cad895af657c6923983b164fe Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Wed, 23 Jun 2021 07:33:11 +0200 Subject: [PATCH] HTE: relax validity condition when already valid (#17783) The condition to get valid requires a low variance and test ratio. but to stay valid, only the variance is required. Co-authored-by: Daniel Agar --- .../MulticopterHoverThrustEstimator.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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();