diff --git a/EKF/control.cpp b/EKF/control.cpp index f2b555d4b8..d4690753e3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1188,7 +1188,15 @@ void Ekf::controlMagFusion() _control_status.flags.mag_hdg = true; } - // perform switch-over from only updating the mag states to updating all states + /* + Control switch-over between only updating the mag states to updating all states + When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. + When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed + to access the magnetic field states. + */ + _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) + && _control_status.flags.fixed_wing; + if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) { // When re-commencing use of magnetometer to correct vehicle states // set the field state variance to the observation variance and zero diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1290ff5c0f..2294d1a5b0 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,14 +163,6 @@ void Ekf::fuseMag() return; } - /* - When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. - When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed - to access the magnetic field states. - */ - _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) - && _control_status.flags.fixed_wing; - // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) {