dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add accel bias estimation fault to reporting

This commit is contained in:
Paul Riseborough 2017-02-28 11:52:05 +11:00 committed by Lorenz Meier
parent b51abf4aff
commit 324fe3b0c7
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -412,6 +412,7 @@ union fault_status_u {
bool bad_pos_N: 1; // 12 - true if fusion of the North 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_acc_bias: 1; // 15 - true if bad delta velocity bias estimates have been detected
} flags;
uint16_t value;

10
EKF/covariance.cpp
View File

@ -715,10 +715,12 @@ void Ekf::fixCovarianceErrors()
&& down_dvel_bias * _vel_pos_innov[2] < 0.0f
&& down_dvel_bias * _vel_pos_innov[5] < 0.0f);
// record the pass and force symmetry on the covariance matrix
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = true;
_time_acc_bias_check = _time_last_imu;
makeSymmetrical(P,13,15);
} else {
_fault_status.flags.bad_acc_bias = true;
}
// if we have failed for 7 seconds continuously, reset the accel bias covariance
@ -732,7 +734,11 @@ void Ekf::fixCovarianceErrors()
P[14][14] = varY;
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P,13,15);
}
}