fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
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_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;

5
EKF/covariance.cpp
View File

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