From 0f260fc26c7bcfff3abb362e2306ac2c09c289c4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 17 Aug 2023 16:51:32 -0400 Subject: [PATCH] ekf2: gravity fusion don't use any accel samples that are clipping --- src/modules/ekf2/EKF/gravity_fusion.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 8a09b72f7999..0017835d9ec7 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -82,7 +82,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; - if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) { + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + + if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) { // perform fusion for each axis _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) && measurementUpdate(Ky, innovation_variance(1), innovation(1))