From 122f58b94d0b77347aab9d2de2b9994761e29979 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 27 Jun 2020 17:52:51 +0200 Subject: [PATCH] Bug: stop overwriting fault status of index 0 with index 1 --- EKF/mag_fusion.cpp | 7 ++++--- EKF/optflow_fusion.cpp | 5 +++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 11ba2e7322..f1f13fbaf1 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -173,6 +173,10 @@ void Ekf::fuseMag() // before they are used to constrain heading drift const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { @@ -365,9 +369,6 @@ void Ekf::fuseMag() // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected - _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)) { diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 866a3abb33..91d4296107 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -428,6 +428,9 @@ void Ekf::fuseOptFlow() } + _fault_status.flags.bad_optflow_X = false; + _fault_status.flags.bad_optflow_Y = false; + for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { // copy the Kalman gain vector for the axis we are fusing @@ -468,8 +471,6 @@ void Ekf::fuseOptFlow() // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool healthy = true; - _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)) {