ekf2: more conservative clipping checks for bad_acc_clipping fault st…

…atus (#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

@@ -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();
 }

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);

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))