From 5e790da634b9ec29baa278b4ef599fdc2e20e9bc Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 9 Dec 2021 13:55:59 +0100 Subject: [PATCH] ekf2: reset the timeout timer in the reset function directly --- src/modules/ekf2/EKF/control.cpp | 2 -- src/modules/ekf2/EKF/ekf_helper.cpp | 3 +++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b0a6c7991e..30962a71f3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -734,8 +734,6 @@ void Ekf::controlHeightSensorTimeouts() // Reset vertical position and velocity states to the last measurement if (request_height_reset) { resetHeight(); - // Reset the timout timer - _time_last_hgt_fuse = _time_last_imu; } } } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 0848eb4d95..fbb30165e4 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -345,6 +345,9 @@ void Ekf::resetHeight() // that does not destabilise the filter P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); } + + // Reset the timout timer + _time_last_hgt_fuse = _time_last_imu; } // align output filter states to match EKF states at the fusion time horizon