diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c16ca24476cc..1571aa06ca86 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 46e03bcaaef4..45917b6348f9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -706,7 +706,7 @@ class Ekf final : public EstimatorInterface // 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 aad5c286b8d3..51da34e343a5 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]); + + _fault_status.flags.bad_acc_clipping = all_axis_warning || any_axis_critical; - const bool is_clipping_frequently = _clip_counter > 0; + // 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))