diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index d817c01ac8..5bc5370f48 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -275,6 +275,15 @@ void Ekf::stopMagFusion() if (_control_status.flags.mag) { ECL_INFO("stopping mag fusion"); + if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) { + // reset yaw alignment from mag unless using GNSS aiding + const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + + if (!using_ne_aiding) { + _control_status.flags.yaw_align = false; + } + } + _control_status.flags.mag = false; _control_status.flags.mag_dec = false; @@ -289,6 +298,8 @@ void Ekf::stopMagFusion() _fault_status.flags.bad_hdg = false; } + _control_status.flags.mag_aligned_in_flight = false; + _fault_status.flags.bad_mag_x = false; _fault_status.flags.bad_mag_y = false; _fault_status.flags.bad_mag_z = false;