From 4cf8eb82267330480b1171de0f0ac23366d7e1d0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 14 Oct 2021 10:32:41 -0400 Subject: [PATCH] ekf2: EKF vehicle_at_rest always require some rotation in addition to vibration metrics --- src/modules/ekf2/EKF/estimator_interface.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 305e5fc547..2466a3200f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -87,9 +87,8 @@ bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu) { // detect if the vehicle is not moving when on ground if (!_control_status.flags.in_air) { - if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler) - || (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler) - || ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { + if (((_vibe_metrics(1) * 4.0e4f > _params.is_moving_scaler) || (_vibe_metrics(2) * 2.1e2f > _params.is_moving_scaler)) + && ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { _time_last_move_detect_us = imu.time_us; }