ekf control: remove AUTOFW mag fusion type as not needed This was imp…

…lemented for VTOL but did not solve the problem and should not be used anymore

EKF/common.h

@@ -200,7 +200,7 @@ struct auxVelSample {
 #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.
+#define MAG_FUSE_TYPE_UNUSED    3	///< Not implemented
 #define MAG_FUSE_TYPE_INDOOR    4	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
 #define MAG_FUSE_TYPE_NONE	5	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 

EKF/ekf.cpp

@@ -261,7 +261,7 @@ bool Ekf::initialiseFilter()
 		if (_control_status.flags.ev_yaw) {
 			// using error estimate from external vision data TODO: this is never true
 			increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// using magnetic heading tuning parameter
 			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
 		}

EKF/ekf.h

@@ -637,7 +637,6 @@ class Ekf : public EstimatorInterface
 	bool isMagBiasObservable() const;
 	bool canUse3DMagFusion() const;
 
-	void controlMagStateOnlyFusion();
 	void checkMagDeclRequired();
 	void checkMagInhibition();
 	bool shouldInhibitMag() const;

EKF/ekf_helper.cpp

@@ -589,7 +589,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 			// calculate the yaw angle for a 312 sequence
 			euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
 
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			Vector3f mag_earth_pred = R_to_earth * mag_init;
 			// the angle of the projection onto the horizontal gives the yaw angle
@@ -647,7 +647,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 			// calculate the yaw angle for a 312 sequence
 			euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
 
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			Vector3f mag_earth_pred = R_to_earth * mag_init;
 			// the angle of the projection onto the horizontal gives the yaw angle
@@ -723,7 +723,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		if (_control_status.flags.ev_yaw) {
 			// using error estimate from external vision data
 			increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// using magnetic heading tuning parameter
 			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
 		}

EKF/mag_control.cpp

@@ -76,11 +76,6 @@ void Ekf::controlMagFusion()
 			selectMagAuto();
 			break;
 
-		case MAG_FUSE_TYPE_AUTOFW:
-			selectMagAuto();
-			controlMagStateOnlyFusion();
-			break;
-
 		case MAG_FUSE_TYPE_INDOOR:
 		/* fallthrough */
 		case MAG_FUSE_TYPE_HEADING:
@@ -92,7 +87,7 @@ void Ekf::controlMagFusion()
 			break;
 
 		default:
-			stopMagFusion();
+			selectMagAuto();
 			break;
 		}
 
@@ -251,37 +246,6 @@ bool Ekf::canUse3DMagFusion() const
 	       && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
 }
 
-void Ekf::controlMagStateOnlyFusion()
-{
-	/*
-	Control switch-over between only updating the mag states to updating all states
-	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, then magnetometer fusion is only allowed
-	to access the magnetic field states.
-	*/
-	_control_status.flags.update_mag_states_only =  _control_status.flags.fixed_wing;
-
-	// For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
-	// before they are used to constrain heading drift
-	_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6);
-
-	if (_control_status.flags.mag_3D
-	    && _control_status_prev.flags.update_mag_states_only
-	    && !_control_status.flags.update_mag_states_only) {
-		// When re-commencing use of magnetometer to correct vehicle states
-		// set the field state variance to the observation variance and zero
-		// the covariance terms to allow the field states re-learn rapidly
-		clearMagCov();
-
-		for (uint8_t index = 0; index <= 5; index ++) {
-			P[index + 16][index + 16] = sq(_params.mag_noise);
-		}
-
-		// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-		saveMagCovData();
-	}
-}
-
 void Ekf::checkMagDeclRequired()
 {
 	// if we are using 3-axis magnetometer fusion, but without external NE aiding,