From e2955bdd61eecdaa5d39d5ab3db5605edd55ab32 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 3 Jun 2022 10:16:27 +0200 Subject: [PATCH] terrain est: clear innovation/var/test ration when aiding stops --- src/modules/ekf2/EKF/terrain_estimator.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 3cb1c76080..14255f6d71 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -194,6 +194,10 @@ void Ekf::resetHaglRng() void Ekf::stopHaglRngFusion() { _hagl_sensor_status.flags.range_finder = false; + _hagl_innov = 0.f; + _hagl_innov_var = 0.f; + _hagl_test_ratio = 0.f; + _innov_check_fail_status.flags.reject_hagl = false; } void Ekf::fuseHaglRng() @@ -290,6 +294,10 @@ void Ekf::startHaglFlowFusion() void Ekf::stopHaglFlowFusion() { _hagl_sensor_status.flags.flow = false; + _hagl_innov = 0.f; + _hagl_innov_var = 0.f; + _hagl_test_ratio = 0.f; + _innov_check_fail_status.flags.reject_hagl = false; } void Ekf::resetHaglFlow()