From aa5fdd3bb3c9db7840721b331b6fbfd367be8883 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Nov 2024 10:39:33 +0100 Subject: [PATCH] ekf2-rng-kin: allow invalidating at any vertical speed The issue is that when the HAGL is low, the drone will usually decelerate and then the check would run anymore. If the low HAGL estimate is due to bad sensor readings (e.g.: reflections), it will be stuck in that state. --- .../range_finder/range_finder_consistency_check.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 918ab523c0..e6b520b600 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -74,11 +74,6 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { - if (fabsf(vz) < _min_vz_for_valid_consistency) { - // We can only check consistency if there is vertical motion - return; - } - if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { _is_kinematically_consistent = false; @@ -86,7 +81,8 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) } } else { - if ((_test_ratio < 1.f) + if ((fabsf(vz) > _min_vz_for_valid_consistency) + && (_test_ratio < 1.f) && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) ) { _is_kinematically_consistent = true;