ekf2: Add new mag fusion mode

Adds a mode where mag fusion is only used update the field estimates

EKF/common.h

@@ -179,6 +179,7 @@ struct dragSample {
 #define MAG_FUSE_TYPE_AUTO      0	///< The selection of either heading or 3D magnetometer fusion will be automatic
 #define MAG_FUSE_TYPE_HEADING   1	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
 #define MAG_FUSE_TYPE_3D        2	///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
+#define MAG_FUSE_TYPE_AUTOFW    3	///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states.
 
 // Maximum sensor intervals in usec
 #define GPS_MAX_INTERVAL	5e5	///< Maximum allowable time interval between GPS measurements (uSec)

EKF/control.cpp

@@ -1105,7 +1105,7 @@ void Ekf::controlMagFusion()
 			// do no magnetometer fusion at all
 			_control_status.flags.mag_hdg = false;
 			_control_status.flags.mag_3D = false;
-		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
+		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) {
 			// Check if height has increased sufficiently to be away from ground magnetic anomalies
 			bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
 

EKF/mag_fusion.cpp

@@ -163,6 +163,15 @@ 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 as detected by the use of wind estimation
+	and current airspeed estimates, 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.wind
+			&& (_time_last_imu - _time_last_airspeed < 1E7);
+
 	// update the states and covariance using sequential fusion of the magnetometer components
 	for (uint8_t index = 0; index <= 2; index++) {