EKF: add fault status bit for clipping

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

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/EKF/common.h b/EKF/common.h
@@ -401,6 +401,7 @@ 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)
 	} flags;
 	uint16_t value;
 

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