EKF: Fix bug causing continual yaw reset when EKF2_MAG_TYPE = 2

EKF/control.cpp

@@ -1533,16 +1533,10 @@ void Ekf::controlMagFusion()
 			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
-			// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
-			if (!_control_status.flags.mag_3D || !_control_status.flags.mag_align_complete) {
-				_control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
-				_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete;
+			if (!_control_status.flags.mag_3D && _control_status.flags.yaw_align && (_flt_mag_align_start_time > 0)) {
+				// only commence 3-axis fusion when yaw is aligned and field states set
+				_control_status.flags.mag_3D = true;
 			}
-
-			// use 3-axis mag fusion if reset was successful
-			_control_status.flags.mag_3D = _control_status.flags.mag_align_complete;
-			_control_status.flags.mag_hdg = false;
-
 		} else {
 			// do no magnetometer fusion at all
 			_control_status.flags.mag_hdg = false;