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