From 408cf011b2f1adc215ac1f7dca6d6a452437171c Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 Dec 2021 16:35:19 +0100 Subject: [PATCH] failure detector: fix dt computation --- src/modules/commander/failure_detector/FailureDetector.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index ed02808e6e..017da04b94 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -191,6 +191,8 @@ void FailureDetector::updateImbalancedPropStatus() if ((imu_status.accel_device_id != 0) && (imu_status.accel_device_id == _selected_accel_device_id)) { const float dt = math::constrain((float)(imu_status.timestamp - _imu_status_timestamp_prev), 0.01f, 1.f); + _imu_status_timestamp_prev = imu_status.timestamp; + _imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant); const float var_x = imu_status.var_accel[0];