EKF: add fault status bit for bad vertical accel data

PX4 · Oct 26, 2020 · 6e99ebd · 6e99ebd
1 parent d936b85
commit 6e99ebd
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 8 deletions.
diff --git a/EKF/common.h b/EKF/common.h
@@ -401,7 +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_clipping: 1; ///< 16 - true if delta velocity data contains clipping (asymmetric railing)
+		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;
 

diff --git a/EKF/control.cpp b/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;
 	}
 }
 

diff --git a/EKF/covariance.cpp b/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;

diff --git a/EKF/ekf.h b/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

diff --git a/EKF/ekf_helper.cpp b/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;
 }