From 0742d356f53190ef9f3705026da50327717237f8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Jun 2024 16:45:08 -0400 Subject: [PATCH] ekf2: more conservative clipping checks for bad_acc_clipping fault status (#23337) - track accel clipping count per axis - only set bad_acc_clipping fault_status if at least one axis is clipping continuously or if all have been clipping at warning level - Note: this doesn't impact the clipping projections that boost the accel process noise, pause bias estimation, and skip gravity fusion on a per sample basis --- src/modules/ekf2/EKF/ekf.cpp | 5 +++- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/height_control.cpp | 32 ++++++++++++++++--------- 3 files changed, 26 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c16ca24476..1571aa06ca 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -127,7 +127,10 @@ void Ekf::reset() _time_bad_vert_accel = 0; _time_good_vert_accel = 0; - _clip_counter = 0; + + for (auto &clip_count : _clip_counter) { + clip_count = 0; + } _zero_velocity_update.reset(); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 46e03bcaae..45917b6348 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -706,7 +706,7 @@ private: // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) - uint16_t _clip_counter{0}; ///< counter that increments when clipping ad decrements when not + uint16_t _clip_counter[3]; ///< counter per axis that increments when clipping ad decrements when not // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index aad5c286b8..51da34e343 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -230,22 +230,32 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood(); - // Check for more than 50% clipping affected IMU samples within the past 1 second - const uint16_t clip_count_limit = 1.f / _dt_ekf_avg; - const bool is_clipping = imu_delayed.delta_vel_clipping[0] || - imu_delayed.delta_vel_clipping[1] || - imu_delayed.delta_vel_clipping[2]; + const uint16_t kClipCountLimit = 1.f / _dt_ekf_avg; - if (is_clipping && _clip_counter < clip_count_limit) { - _clip_counter++; + bool acc_clip_warning[3] {}; + bool acc_clip_critical[3] {}; - } else if (_clip_counter > 0) { - _clip_counter--; + for (int axis = 0; axis < 3; axis++) { + if (imu_delayed.delta_vel_clipping[axis] && (_clip_counter[axis] < kClipCountLimit)) { + _clip_counter[axis]++; + + } else if (_clip_counter[axis] > 0) { + _clip_counter[axis]--; + } + + // warning if more than 50% clipping affected IMU samples within the past 1 second + acc_clip_warning[axis] = _clip_counter[axis] >= kClipCountLimit / 2; + acc_clip_critical[axis] = _clip_counter[axis] >= kClipCountLimit; } - _fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2; + // bad_acc_clipping if ALL axes are reporting warning or if ANY axis is critical + const bool all_axis_warning = (acc_clip_warning[0] && acc_clip_warning[1] && acc_clip_warning[2]); + const bool any_axis_critical = (acc_clip_critical[0] || acc_clip_critical[1] || acc_clip_critical[2]); - const bool is_clipping_frequently = _clip_counter > 0; + _fault_status.flags.bad_acc_clipping = all_axis_warning || any_axis_critical; + + // if Z axis is warning or any other axis critical + const bool is_clipping_frequently = acc_clip_warning[2] || _fault_status.flags.bad_acc_clipping; // Do not require evidence of clipping if the likelihood of having the INS falling is high const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM))