From 020b87933ebb3d908bfa4a48445b4890f4da0f09 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 7 May 2016 21:11:16 +1000 Subject: [PATCH] EKF: replace fault status struct with a union to facilitate logging --- EKF/airspeed_fusion.cpp | 4 ++-- EKF/common.h | 35 +++++++++++++++++++---------------- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 2 +- EKF/mag_fusion.cpp | 36 ++++++++++++++++++------------------ EKF/optflow_fusion.cpp | 8 ++++---- EKF/vel_pos_fusion.cpp | 24 ++++++++++++------------ 7 files changed, 57 insertions(+), 54 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index cf7946afb9..8a3c52c87a 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -161,7 +161,7 @@ void Ekf::fuseAirspeed() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; - _fault_status.bad_airspeed = false; + _fault_status.flags.bad_airspeed = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -172,7 +172,7 @@ void Ekf::fuseAirspeed() healthy = false; // update individual measurement health status - _fault_status.bad_airspeed = true; + _fault_status.flags.bad_airspeed = true; } } diff --git a/EKF/common.h b/EKF/common.h index 1eb3ef2c98..099dae505c 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -322,22 +322,25 @@ struct stateSample { Vector2f wind_vel; // wind velocity in m/s }; -struct fault_status_t { - bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error - bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-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_hdg: 1; // true if the fusion of the magnetic heading 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_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint 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_optflow_Y: 1; // true if fusion of the optical flow Y 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_vel_E: 1; // true if fusion of the East 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_pos_N: 1; // true if fusion of the North position 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_D: 1; // true if fusion of the Down position has encountered a numerical error +union fault_status_u { + struct { + bool bad_mag_x: 1; // 0 - true if the fusion of the magnetometer X-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_z: 1; // 2 - true if the fusion of the magnetometer Z-axis 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_mag_decl: 1; // 4 - true if the fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed: 1; // 5 - true if fusion of the airspeed 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_X: 1; // 7 - true if fusion of the optical flow X axis 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_N: 1; // 9 - true if fusion of the North 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_vel_D: 1; // 11 - true if fusion of the Down velocity 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_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; }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a4dfcee8c8..162cfaba21 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -346,7 +346,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _time_last_airspeed = 0; _time_last_optflow = 0; - memset(&_fault_status, 0, sizeof(_fault_status)); + memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); return true; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1793dbd939..191f1a0cd9 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -286,7 +286,7 @@ protected: uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds uint64_t _time_last_optflow; - fault_status_t _fault_status; + fault_status_u _fault_status; // allocate data buffers and intialise interface variables bool initialise_interface(uint64_t timestamp); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index ced31afb01..88c133deae 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -105,11 +105,11 @@ void Ekf::fuseMag() // check for a badly conditioned covariance matrix if (_mag_innov_var[0] >= R_MAG) { // 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 { // 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 initialiseCovariance(); return; @@ -134,11 +134,11 @@ void Ekf::fuseMag() // check for a badly conditioned covariance matrix if (_mag_innov_var[1] >= R_MAG) { // 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 { // 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 initialiseCovariance(); return; @@ -163,11 +163,11 @@ void Ekf::fuseMag() // check for a badly conditioned covariance matrix if (_mag_innov_var[2] >= R_MAG) { // 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 { // 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 initialiseCovariance(); return; @@ -329,9 +329,9 @@ void Ekf::fuseMag() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; - _fault_status.bad_mag_x = false; - _fault_status.bad_mag_y = false; - _fault_status.bad_mag_z = false; + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -343,11 +343,11 @@ void Ekf::fuseMag() // update individual measurement health status if (index == 0) { - _fault_status.bad_mag_x = true; + _fault_status.flags.bad_mag_x = true; } else if (index == 1) { - _fault_status.bad_mag_y = true; + _fault_status.flags.bad_mag_y = true; } 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 if (_heading_innov_var >= R_YAW) { // 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; } else { // 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 initialiseCovariance(); @@ -610,7 +610,7 @@ void Ekf::fuseHeading() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected 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++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -621,7 +621,7 @@ void Ekf::fuseHeading() healthy = false; // 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 // the covariance marix is unhealthy and must be corrected 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++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -755,7 +755,7 @@ void Ekf::fuseDeclination() healthy = false; // update individual measurement health status - _fault_status.bad_mag_decl = true; + _fault_status.flags.bad_mag_decl = true; } } diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index cc9d398dd4..660024cf39 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -456,8 +456,8 @@ void Ekf::fuseOptFlow() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; - _fault_status.bad_optflow_X = false; - _fault_status.bad_optflow_Y = false; + _fault_status.flags.bad_optflow_X = false; + _fault_status.flags.bad_optflow_Y = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -469,9 +469,9 @@ void Ekf::fuseOptFlow() // update individual measurement health status if (obs_index == 0) { - _fault_status.bad_optflow_X = true; + _fault_status.flags.bad_optflow_X = true; } else if (obs_index == 1) { - _fault_status.bad_optflow_Y = true; + _fault_status.flags.bad_optflow_Y = true; } } } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index f638647fdb..c941db89e5 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -222,32 +222,32 @@ void Ekf::fuseVelPosHeight() // update individual measurement health status if (obs_index == 0) { - _fault_status.bad_vel_N = true; + _fault_status.flags.bad_vel_N = true; } else if (obs_index == 1) { - _fault_status.bad_vel_E = true; + _fault_status.flags.bad_vel_E = true; } else if (obs_index == 2) { - _fault_status.bad_vel_D = true; + _fault_status.flags.bad_vel_D = true; } else if (obs_index == 3) { - _fault_status.bad_pos_N = true; + _fault_status.flags.bad_pos_N = true; } else if (obs_index == 4) { - _fault_status.bad_pos_E = true; + _fault_status.flags.bad_pos_E = true; } else if (obs_index == 5) { - _fault_status.bad_pos_D = true; + _fault_status.flags.bad_pos_D = true; } } else { // update individual measurement health status if (obs_index == 0) { - _fault_status.bad_vel_N = false; + _fault_status.flags.bad_vel_N = false; } else if (obs_index == 1) { - _fault_status.bad_vel_E = false; + _fault_status.flags.bad_vel_E = false; } else if (obs_index == 2) { - _fault_status.bad_vel_D = false; + _fault_status.flags.bad_vel_D = false; } else if (obs_index == 3) { - _fault_status.bad_pos_N = false; + _fault_status.flags.bad_pos_N = false; } else if (obs_index == 4) { - _fault_status.bad_pos_E = false; + _fault_status.flags.bad_pos_E = false; } else if (obs_index == 5) { - _fault_status.bad_pos_D = false; + _fault_status.flags.bad_pos_D = false; } } }