EKF: add fault status bit for clipping

EKF/common.h

@@ -401,6 +401,8 @@ union fault_status_u {
 		bool bad_pos_E: 1;	///< 13 - true if fusion of the East position has encountered a numerical error
 		bool bad_pos_D: 1;	///< 14 - true if fusion of the Down position has encountered a numerical error
 		bool bad_acc_bias: 1;	///< 15 - true if bad delta velocity bias estimates have been detected
+		bool bad_acc_vertical: 1; ///< 16 - true if bad vertical accelerometer data has been detected
+		bool bad_acc_clipping: 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
 	uint16_t value;
 

EKF/control.cpp

@@ -898,11 +898,11 @@ void Ekf::checkVerticalAccelerationHealth()
 
 	// declare a bad vertical acceleration measurement and make the declaration persist
 	// for a minimum of BADACC_PROBATION seconds
-	if (_bad_vert_accel_detected) {
-		_bad_vert_accel_detected = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
+	if (_fault_status.flags.bad_acc_vertical) {
+		_fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
 
 	} else {
-		_bad_vert_accel_detected = bad_vert_accel;
+		_fault_status.flags.bad_acc_vertical = bad_vert_accel;
 	}
 }
 

EKF/covariance.cpp

@@ -160,7 +160,7 @@ void Ekf::predictCovariance()
 
 	const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)
 					 || is_manoeuvre_level_high
-					 || _bad_vert_accel_detected;
+					 || _fault_status.flags.bad_acc_vertical;
 
 	for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) {
 		const unsigned index = stateIndex - 13;
@@ -245,7 +245,7 @@ void Ekf::predictCovariance()
 
 	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 
-	if (_bad_vert_accel_detected) {
+	if (_fault_status.flags.bad_acc_vertical) {
 		// Increase accelerometer process noise if bad accel data is detected. Measurement errors due to
 		// vibration induced clipping commonly reach an equivalent 0.5g offset.
 		accel_noise = BADACC_BIAS_PNOISE;
@@ -255,17 +255,22 @@ 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

EKF/ekf.h

@@ -518,7 +518,6 @@ class Ekf : 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)
-	bool _bad_vert_accel_detected{false};	///< true when bad vertical accelerometer data has been detected
 
 	// variables used to control range aid functionality
 	bool _is_range_aid_suitable{false};	///< true when range finder can be used in flight as the height reference instead of the primary height sensor

EKF/ekf_helper.cpp

@@ -1092,7 +1092,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
 	const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
 	const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
-	soln_status.flags.accel_error = _bad_vert_accel_detected;
+	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
 	*status = soln_status.value;
 }