EKF: Improve resistance to bad initial mag offset

When magnetic field states have been reset in-flight using a single sample, the magnetic field states are not used to constrain heading drift for a period after the reset. This period has been shortened from 10 to 5 seconds which is enough time to average out the effects of measurement noise (the original concern). The shorter time has enabled the previous practice for RW vehicles of using magnetic heading in that time period to constrain yaw drift to be discontinued. This is necessary becasue while magnetic heading is being used, it fights the yaw corrections obtained from GPs observations and lengthens the time required to recover from a bad mag calibration.

EKF/control.cpp

@@ -1244,8 +1244,9 @@ void Ekf::controlMagFusion()
 			_control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW)
 					&& _control_status.flags.fixed_wing;
 
-			// For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
-			_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7);
+			// 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.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
 				// When re-commencing use of magnetometer to correct vehicle states
@@ -1296,12 +1297,6 @@ void Ekf::controlMagFusion()
 				fuseDeclination();
 			}
 
-			if (_flt_mag_align_converging && !_control_status.flags.fixed_wing) {
-				_control_status.flags.mag_hdg = true;
-				fuseHeading();
-				_control_status.flags.mag_hdg = false;
-			}
-
 		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
 			// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
 			fuseHeading();