Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

EKF: add fault status bit for clipping

Browse Source
This commit is contained in:
Daniel Agar
2020-10-21 13:15:04 -04:00
committed by Mathieu Bresciani
parent dd3ffc4192
commit d936b85d71
2 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+1
View File
@@ -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_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_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_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; } flags;
uint16_t value; uint16_t value;
EKF/covariance.cpp
+5
View File
@@ -255,17 +255,22 @@ void Ekf::predictCovariance()
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
// Accelerometer Clipping // Accelerometer Clipping
_fault_status.flags.bad_acc_clipping = false; // reset flag
// delta velocity X: increase process noise if sample contained any X axis clipping // delta velocity X: increase process noise if sample contained any X axis clipping
if (_imu_sample_delayed.delta_vel_clipping[0]) { if (_imu_sample_delayed.delta_vel_clipping[0]) {
dvxVar = sq(dt * BADACC_BIAS_PNOISE); 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 // delta velocity Y: increase process noise if sample contained any Y axis clipping
if (_imu_sample_delayed.delta_vel_clipping[1]) { if (_imu_sample_delayed.delta_vel_clipping[1]) {
dvyVar = sq(dt * BADACC_BIAS_PNOISE); 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 // delta velocity Z: increase process noise if sample contained any Z axis clipping
if (_imu_sample_delayed.delta_vel_clipping[2]) { if (_imu_sample_delayed.delta_vel_clipping[2]) {
dvzVar = sq(dt * BADACC_BIAS_PNOISE); dvzVar = sq(dt * BADACC_BIAS_PNOISE);
_fault_status.flags.bad_acc_clipping = true;
} }
// predict the covariance // predict the covariance