From 49d9d1c9873ed8a731882b429061079243074118 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 25 Jan 2022 12:31:58 -0500 Subject: [PATCH] ekf2: only set fault status bad_acc_clipping if clipping frequently --- src/modules/ekf2/EKF/control.cpp | 2 ++ src/modules/ekf2/EKF/covariance.cpp | 5 ----- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 627acf43f6..37360967a1 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -812,6 +812,8 @@ void Ekf::checkVerticalAccelerationHealth() _clip_counter--; } + _fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2; + const bool is_clipping_frequently = _clip_counter > 0; // if vertical velocity and position are independent and agree, then do not require evidence of clipping if diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index e0c148dda8..e1dbec9c4b 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -237,24 +237,19 @@ void Ekf::predictCovariance() dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); // Accelerometer Clipping - _fault_status.flags.bad_acc_clipping = false; // reset flag - // delta velocity X: increase process noise if sample contained any X axis clipping if (_imu_sample_delayed.delta_vel_clipping[0]) { dvxVar = sq(dt * BADACC_BIAS_PNOISE); - _fault_status.flags.bad_acc_clipping = true; } // delta velocity Y: increase process noise if sample contained any Y axis clipping if (_imu_sample_delayed.delta_vel_clipping[1]) { dvyVar = sq(dt * BADACC_BIAS_PNOISE); - _fault_status.flags.bad_acc_clipping = true; } // delta velocity Z: increase process noise if sample contained any Z axis clipping if (_imu_sample_delayed.delta_vel_clipping[2]) { dvzVar = sq(dt * BADACC_BIAS_PNOISE); - _fault_status.flags.bad_acc_clipping = true; } // predict the covariance