From 0220f469b0c759065f8cb0ba74bf9519f868c933 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 18 Sep 2018 19:59:02 +1000 Subject: [PATCH] EKF: Use more generic variable name for bad yaw fusion flag This flag now reports on fusion of data that is not from a magnetometer. --- EKF/common.h | 2 +- EKF/gps_yaw_fusion.cpp | 8 ++++---- EKF/mag_fusion.cpp | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 973975854c..7fee654bd9 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -367,7 +367,7 @@ union fault_status_u { 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_hdg: 1; ///< 3 - true if the fusion of the heading angle 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 diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 8a2a3cc601..db4ade8175 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -165,12 +165,12 @@ void Ekf::fuseGpsAntYaw() // 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.flags.bad_mag_hdg = false; + _fault_status.flags.bad_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.flags.bad_mag_hdg = true; + _fault_status.flags.bad_hdg = true; // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); @@ -255,7 +255,7 @@ void Ekf::fuseGpsAntYaw() // 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.flags.bad_mag_hdg = false; + _fault_status.flags.bad_hdg = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { @@ -267,7 +267,7 @@ void Ekf::fuseGpsAntYaw() healthy = false; // update individual measurement health status - _fault_status.flags.bad_mag_hdg = true; + _fault_status.flags.bad_hdg = true; } } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index eff0a4c2b0..7c28c76989 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -666,12 +666,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.flags.bad_mag_hdg = false; + _fault_status.flags.bad_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.flags.bad_mag_hdg = true; + _fault_status.flags.bad_hdg = true; // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); @@ -756,7 +756,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.flags.bad_mag_hdg = false; + _fault_status.flags.bad_hdg = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { @@ -768,7 +768,7 @@ void Ekf::fuseHeading() healthy = false; // update individual measurement health status - _fault_status.flags.bad_mag_hdg = true; + _fault_status.flags.bad_hdg = true; } }