From 2c76109a9462347628d65a8e820d86a1b3633dcd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 17 Aug 2023 16:16:10 -0400 Subject: [PATCH] ekf2: drag fusion don't use any accel samples that are clipping --- src/modules/ekf2/EKF/estimator_interface.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 8e780a4f33..ab63ae5f7a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -494,6 +494,17 @@ void EstimatorInterface::setDragData(const imuSample &imu) } } + // don't use any accel samples that are clipping + if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) { + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + + return; + } + _drag_sample_count++; // note acceleration is accumulated as a delta velocity _drag_down_sampled.accelXY(0) += imu.delta_vel(0);