diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index e575dce2ec..2829c46154 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -427,16 +427,9 @@ void NavEKF3_core::SelectMagFusion() (frontend->_mag_ef_limit > 0 && have_table_earth_field)) { FuseDeclination(0.34f); } - // fuse the three magnetometer componenents sequentially + // fuse the three magnetometer componenents using sequential fusion for each axis hal.util->perf_begin(_perf_test[0]); - for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) { - FuseMagnetometer(); - // don't continue fusion if unhealthy - if (!magHealth) { - hal.util->perf_end(_perf_test[0]); - break; - } - } + FuseMagnetometer(); hal.util->perf_end(_perf_test[0]); // zero the test ratio output from the inactive simple magnetometer yaw fusion yawTestRatio = 0.0f; @@ -481,7 +474,6 @@ void NavEKF3_core::FuseMagnetometer() ftype &magXbias = mag_state.magXbias; ftype &magYbias = mag_state.magYbias; ftype &magZbias = mag_state.magZbias; - uint8_t &obsIndex = mag_state.obsIndex; Matrix3f &DCM = mag_state.DCM; Vector3f &MagPred = mag_state.MagPred; ftype &R_MAG = mag_state.R_MAG; @@ -595,7 +587,7 @@ void NavEKF3_core::FuseMagnetometer() return; } - for (obsIndex = 0; obsIndex <= 2; obsIndex++) { + for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) { if (obsIndex == 0) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2d22718ba7..c607406cd4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1389,7 +1389,6 @@ private: ftype magXbias; ftype magYbias; ftype magZbias; - uint8_t obsIndex; Matrix3f DCM; Vector3f MagPred; ftype R_MAG;