EKF: Ensure normal yaw alignment using mag is performed if GPS method is not

EKF/control.cpp

@@ -1050,7 +1050,7 @@ void Ekf::controlMagFusion()
 				if (!_control_status.flags.mag_3D) {
 					if (!_flt_mag_align_complete) {
 						// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
-						if (_control_status.flags.fixed_wing) {
+						if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
 							_control_status.flags.yaw_align = realignYawGPS();
 							_flt_mag_align_complete = _control_status.flags.yaw_align;
 						} else {
@@ -1069,9 +1069,9 @@ void Ekf::controlMagFusion()
 					}
 				}
 
-				// use 3D mag fusion when airborne
-				_control_status.flags.mag_hdg = false;
-				_control_status.flags.mag_3D = true;
+				// only use one type of mag fusion at the same time
+				_control_status.flags.mag_3D = _flt_mag_align_complete;
+				_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
 
 			} else {
 				// save magnetic field state variances for next time

EKF/ekf_helper.cpp

@@ -361,6 +361,8 @@ bool Ekf::realignYawGPS()
 
 		// correct yaw angle using GPS ground course if compass yaw bad
 		if (badMagYaw) {
+			ECL_WARN("EKF bad yaw corrected using GPS course");
+
 			// save a copy of the quaternion state for later use in calculating the amount of reset change
 			Quatf quat_before_reset = _state.quat_nominal;
 
@@ -459,11 +461,17 @@ bool Ekf::realignYawGPS()
 			// capture the reset event
 			_state_reset_status.quat_counter++;
 
-		}
+			// the alignment using GPS has been successful
+			return true;
 
-		return true;
+		} else {
+			// attempt a normal alignment using the magnetometer
+			return resetMagHeading(_mag_sample_delayed.mag);
+
+		}
 
 	} else {
+		// we were unable to verify the alignment
 		return false;
 
 	}