EKF: delay commencement of 3D mag fusion until clear of ground

Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation.

EKF/control.cpp

@@ -552,6 +552,11 @@ void Ekf::controlHeightAiding()
 		_control_status.flags.ev_hgt = true;
 
 	}
+
+	// If we are on ground, store the local position and time to use as a reference for takeoff checks
+	if (!_control_status.flags.in_air) {
+		_last_on_ground_posD = _state.pos(2);
+	}
 }
 
 void Ekf::controlMagAiding()
@@ -564,22 +569,27 @@ void Ekf::controlMagAiding()
 	// 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 (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
+		// start 3D fusion if in-flight and height has increased sufficiently
+		// to be away from ground magnetic anomalies
+		// don't switch back to heading fusion until we are back on the ground
+		bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
+		bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
+
+		if (use_3D_fusion && _control_status.flags.tilt_align) {
+			// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
+			if (!_control_status.flags.mag_3D) {
+				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			}
 
-	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-		// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
-		if (!_control_status.flags.mag_3D) {
-			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
-		}
-
-		// use 3D mag fusion when airborne
-		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = true;
+			// use 3D mag fusion when airborne
+			_control_status.flags.mag_hdg = false;
+			_control_status.flags.mag_3D = true;
 
-	} else {
-		// use heading fusion when on the ground
-		_control_status.flags.mag_hdg = true;
-		_control_status.flags.mag_3D = false;
-	}
+		} else {
+			// use 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 heading fusion

EKF/ekf.cpp

@@ -98,6 +98,8 @@ Ekf::Ekf():
 	_ev_counter(0),
 	_time_last_mag(0),
 	_hgt_sensor_offset(0.0f),
+	_baro_hgt_offset(0.0f),
+	_last_on_ground_posD(0.0f),
 	_terrain_vpos(0.0f),
 	_terrain_var(1.e4f),
 	_hagl_innov(0.0f),
@@ -106,7 +108,6 @@ Ekf::Ekf():
 	_baro_hgt_faulty(false),
 	_gps_hgt_faulty(false),
 	_rng_hgt_faulty(false),
-	_baro_hgt_offset(0.0f),
 	_time_bad_vert_accel(0)
 {
 	_state = {};

EKF/ekf.h

@@ -248,6 +248,10 @@ private:
 	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
 	float _hgt_sensor_offset;	// set as necessary if desired to maintain the same height after a height reset (m)
+	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
+
+	// Variables used to control activation of post takeoff functionality
+	float _last_on_ground_posD; // last vertical position when the in_air status was false (m)
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
@@ -265,8 +269,6 @@ private:
 	bool _rng_hgt_faulty;		// true if valid rnage finder height data is unavailable for use
 	int _primary_hgt_source;	// priary source of height data set at initialisation
 
-	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
-
 	// imu fault status
 	uint64_t _time_bad_vert_accel;	// last time a bad vertical accel was detected (usec)