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

EKF: replace fault status struct with a union to facilitate logging

This commit is contained in:
Paul Riseborough 2016-05-07 21:11:16 +10:00
parent 7f5669fb2d
commit 020b87933e
7 changed files with 57 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/airspeed_fusion.cpp
View File

@ -161,7 +161,7 @@ void Ekf::fuseAirspeed()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.bad_airspeed = false; _fault_status.flags.bad_airspeed = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
// zero rows and columns // zero rows and columns
@ -172,7 +172,7 @@ void Ekf::fuseAirspeed()
healthy = false; healthy = false;
// update individual measurement health status // update individual measurement health status
_fault_status.bad_airspeed = true; _fault_status.flags.bad_airspeed = true;
} }
} }

35
EKF/common.h
View File

@ -322,22 +322,25 @@ struct stateSample {
Vector2f wind_vel; // wind velocity in m/s Vector2f wind_vel; // wind velocity in m/s
}; };
struct fault_status_t { union fault_status_u {
bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error struct {
bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error bool bad_mag_x: 1; // 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error bool bad_mag_y: 1; // 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error bool bad_mag_z: 1; // 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error bool bad_mag_hdg: 1; // 3 - true if the fusion of the magnetic heading has encountered a numerical error
bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error bool bad_mag_decl: 1; // 4 - true if the fusion of the magnetic declination has encountered a numerical error
bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_airspeed: 1; // 5 - true if fusion of the airspeed has encountered a numerical error
bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error bool bad_sideslip: 1; // 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error bool bad_optflow_X: 1; // 7 - true if fusion of the optical flow X axis has encountered a numerical error
bool bad_vel_N: 1; // true if fusion of the North velocity has encountered a numerical error bool bad_optflow_Y: 1; // 8 - true if fusion of the optical flow Y axis has encountered a numerical error
bool bad_vel_E: 1; // true if fusion of the East velocity has encountered a numerical error bool bad_vel_N: 1; // 9 - true if fusion of the North velocity has encountered a numerical error
bool bad_vel_D: 1; // true if fusion of the Down velocity has encountered a numerical error bool bad_vel_E: 1; // 10 - true if fusion of the East velocity has encountered a numerical error
bool bad_pos_N: 1; // true if fusion of the North position has encountered a numerical error bool bad_vel_D: 1; // 11 - true if fusion of the Down velocity has encountered a numerical error
bool bad_pos_E: 1; // true if fusion of the East position has encountered a numerical error bool bad_pos_N: 1; // 12 - true if fusion of the North position has encountered a numerical error
bool bad_pos_D: 1; // true if fusion of the Down 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
} flags;
uint16_t value;
}; };

2
EKF/estimator_interface.cpp
View File

@ -346,7 +346,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_time_last_airspeed = 0; _time_last_airspeed = 0;
_time_last_optflow = 0; _time_last_optflow = 0;
memset(&_fault_status, 0, sizeof(_fault_status)); memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
return true; return true;
} }

2
EKF/estimator_interface.h
View File

@ -286,7 +286,7 @@ protected:
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_optflow; uint64_t _time_last_optflow;
fault_status_t _fault_status; fault_status_u _fault_status;
// allocate data buffers and intialise interface variables // allocate data buffers and intialise interface variables
bool initialise_interface(uint64_t timestamp); bool initialise_interface(uint64_t timestamp);

36
EKF/mag_fusion.cpp
View File

@ -105,11 +105,11 @@ void Ekf::fuseMag()
// check for a badly conditioned covariance matrix // check for a badly conditioned covariance matrix
if (_mag_innov_var[0] >= R_MAG) { if (_mag_innov_var[0] >= R_MAG) {
// the innovation variance contribution from the state covariances is non-negative - no fault // the innovation variance contribution from the state covariances is non-negative - no fault
_fault_status.bad_mag_x = false; _fault_status.flags.bad_mag_x = false;
} else { } else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.bad_mag_x = true; _fault_status.flags.bad_mag_x = true;
// we need to reinitialise the covariance matrix and abort this fusion step // we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
return; return;
@ -134,11 +134,11 @@ void Ekf::fuseMag()
// check for a badly conditioned covariance matrix // check for a badly conditioned covariance matrix
if (_mag_innov_var[1] >= R_MAG) { if (_mag_innov_var[1] >= R_MAG) {
// the innovation variance contribution from the state covariances is non-negative - no fault // the innovation variance contribution from the state covariances is non-negative - no fault
_fault_status.bad_mag_y = false; _fault_status.flags.bad_mag_y = false;
} else { } else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.bad_mag_y = true; _fault_status.flags.bad_mag_y = true;
// we need to reinitialise the covariance matrix and abort this fusion step // we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
return; return;
@ -163,11 +163,11 @@ void Ekf::fuseMag()
// check for a badly conditioned covariance matrix // check for a badly conditioned covariance matrix
if (_mag_innov_var[2] >= R_MAG) { if (_mag_innov_var[2] >= R_MAG) {
// the innovation variance contribution from the state covariances is non-negative - no fault // the innovation variance contribution from the state covariances is non-negative - no fault
_fault_status.bad_mag_z = false; _fault_status.flags.bad_mag_z = false;
} else { } else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
_fault_status.bad_mag_z = true; _fault_status.flags.bad_mag_z = true;
// we need to reinitialise the covariance matrix and abort this fusion step // we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
return; return;
@ -329,9 +329,9 @@ void Ekf::fuseMag()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.bad_mag_x = false; _fault_status.flags.bad_mag_x = false;
_fault_status.bad_mag_y = false; _fault_status.flags.bad_mag_y = false;
_fault_status.bad_mag_z = false; _fault_status.flags.bad_mag_z = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
// zero rows and columns // zero rows and columns
@ -343,11 +343,11 @@ void Ekf::fuseMag()
// update individual measurement health status // update individual measurement health status
if (index == 0) { if (index == 0) {
_fault_status.bad_mag_x = true; _fault_status.flags.bad_mag_x = true;
} else if (index == 1) { } else if (index == 1) {
_fault_status.bad_mag_y = true; _fault_status.flags.bad_mag_y = true;
} else if (index == 2) { } else if (index == 2) {
_fault_status.bad_mag_z = true; _fault_status.flags.bad_mag_z = true;
} }
} }
} }
@ -515,12 +515,12 @@ void Ekf::fuseHeading()
// check if the innovation variance calculation is badly conditioned // check if the innovation variance calculation is badly conditioned
if (_heading_innov_var >= R_YAW) { if (_heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault // the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_hdg = false; _fault_status.flags.bad_mag_hdg = false;
heading_innov_var_inv = 1.0f / _heading_innov_var; heading_innov_var_inv = 1.0f / _heading_innov_var;
} else { } else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.bad_mag_hdg = true; _fault_status.flags.bad_mag_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step // we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
@ -610,7 +610,7 @@ void Ekf::fuseHeading()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.bad_mag_hdg = false; _fault_status.flags.bad_mag_hdg = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
// zero rows and columns // zero rows and columns
@ -621,7 +621,7 @@ void Ekf::fuseHeading()
healthy = false; healthy = false;
// update individual measurement health status // update individual measurement health status
_fault_status.bad_mag_hdg = true; _fault_status.flags.bad_mag_hdg = true;
} }
} }
@ -744,7 +744,7 @@ void Ekf::fuseDeclination()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.bad_mag_decl = false; _fault_status.flags.bad_mag_decl = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
// zero rows and columns // zero rows and columns
@ -755,7 +755,7 @@ void Ekf::fuseDeclination()
healthy = false; healthy = false;
// update individual measurement health status // update individual measurement health status
_fault_status.bad_mag_decl = true; _fault_status.flags.bad_mag_decl = true;
} }
} }

8
EKF/optflow_fusion.cpp
View File

@ -456,8 +456,8 @@ void Ekf::fuseOptFlow()
// if the covariance correction will result in a negative variance, then // if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected // the covariance marix is unhealthy and must be corrected
bool healthy = true; bool healthy = true;
_fault_status.bad_optflow_X = false; _fault_status.flags.bad_optflow_X = false;
_fault_status.bad_optflow_Y = false; _fault_status.flags.bad_optflow_Y = false;
for (int i = 0; i < _k_num_states; i++) { for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) { if (P[i][i] < KHP[i][i]) {
// zero rows and columns // zero rows and columns
@ -469,9 +469,9 @@ void Ekf::fuseOptFlow()
// update individual measurement health status // update individual measurement health status
if (obs_index == 0) { if (obs_index == 0) {
_fault_status.bad_optflow_X = true; _fault_status.flags.bad_optflow_X = true;
} else if (obs_index == 1) { } else if (obs_index == 1) {
_fault_status.bad_optflow_Y = true; _fault_status.flags.bad_optflow_Y = true;
} }
} }
} }

24
EKF/vel_pos_fusion.cpp
View File

@ -222,32 +222,32 @@ void Ekf::fuseVelPosHeight()
// update individual measurement health status // update individual measurement health status
if (obs_index == 0) { if (obs_index == 0) {
_fault_status.bad_vel_N = true; _fault_status.flags.bad_vel_N = true;
} else if (obs_index == 1) { } else if (obs_index == 1) {
_fault_status.bad_vel_E = true; _fault_status.flags.bad_vel_E = true;
} else if (obs_index == 2) { } else if (obs_index == 2) {
_fault_status.bad_vel_D = true; _fault_status.flags.bad_vel_D = true;
} else if (obs_index == 3) { } else if (obs_index == 3) {
_fault_status.bad_pos_N = true; _fault_status.flags.bad_pos_N = true;
} else if (obs_index == 4) { } else if (obs_index == 4) {
_fault_status.bad_pos_E = true; _fault_status.flags.bad_pos_E = true;
} else if (obs_index == 5) { } else if (obs_index == 5) {
_fault_status.bad_pos_D = true; _fault_status.flags.bad_pos_D = true;
} }
} else { } else {
// update individual measurement health status // update individual measurement health status
if (obs_index == 0) { if (obs_index == 0) {
_fault_status.bad_vel_N = false; _fault_status.flags.bad_vel_N = false;
} else if (obs_index == 1) { } else if (obs_index == 1) {
_fault_status.bad_vel_E = false; _fault_status.flags.bad_vel_E = false;
} else if (obs_index == 2) { } else if (obs_index == 2) {
_fault_status.bad_vel_D = false; _fault_status.flags.bad_vel_D = false;
} else if (obs_index == 3) { } else if (obs_index == 3) {
_fault_status.bad_pos_N = false; _fault_status.flags.bad_pos_N = false;
} else if (obs_index == 4) { } else if (obs_index == 4) {
_fault_status.bad_pos_E = false; _fault_status.flags.bad_pos_E = false;
} else if (obs_index == 5) { } else if (obs_index == 5) {
_fault_status.bad_pos_D = false; _fault_status.flags.bad_pos_D = false;
} }
} }
} }