EKF: Add control logic for fusion modes

EKF/control.cpp

@@ -62,8 +62,8 @@ void Ekf::controlFusionModes()
 
 			// If the heading is valid, reset the positon and velocity and start using gps aiding
 			if (_control_status.flags.yaw_align) {
-			resetPosition();
+				resetPosition();
-			resetVelocity();
+				resetVelocity();
 				_control_status.flags.gps = true;
 			}
 		}
@@ -101,31 +101,50 @@ void Ekf::controlFusionModes()
 
 	// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
 	// or the more accurate 3-axis fusion
-	if (!_control_status.flags.armed) {
+	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
-		// always use simple mag fusion for initial startup
+		if (!_control_status.flags.armed) {
+			// always use simple mag fusion for initial startup
+			_control_status.flags.mag_hdg = true;
+			_control_status.flags.mag_3D = false;
+
+		} else {
+			if (_control_status.flags.in_air) {
+				// always use 3-axis mag fusion when airborne
+				_control_status.flags.mag_hdg = false;
+				_control_status.flags.mag_3D = true;
+
+			} else {
+				// always use simple heading fusion when on the ground
+				_control_status.flags.mag_hdg = true;
+				_control_status.flags.mag_3D = false;
+			}
+		}
+
+	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
+		// always use simple heading fusion
 		_control_status.flags.mag_hdg = true;
 		_control_status.flags.mag_3D = false;
 
-	} else {
+	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
-		if (_control_status.flags.in_air) {
+		// always use 3-axis mag fusion
-			// always use 3-axis mag fusion when airborne
+		_control_status.flags.mag_hdg = false;
-			_control_status.flags.mag_hdg = false;
+		_control_status.flags.mag_3D = true;
-			_control_status.flags.mag_3D = true;
 
-		} else {
+	} else {
-			// always use simple heading fusion when on the ground
+		// do no magnetometer fusion at all
-			_control_status.flags.mag_hdg = true;
+		_control_status.flags.mag_hdg = false;
-			_control_status.flags.mag_3D = false;
+		_control_status.flags.mag_3D = false;
-		}
 	}
 
-	// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination needs to be fused as an observation to prevent long term heading drift
+	// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
-	if (_control_status.flags.mag_3D && !_control_status.flags.gps) {
+	// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
+	if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
 		_control_status.flags.mag_dec = true;
 
 	} else {
 		_control_status.flags.mag_dec = false;
 	}
+
 }
 
 void Ekf::calculateVehicleStatus()