Merge pull request PX4#42 from wingtra/pr-new_mag_type

EKF: force 3D axis fusion in hover above 1.5 meter altitude otherwise…

EKF/control.cpp

@@ -1163,7 +1163,8 @@ void Ekf::controlMagFusion()
 			_control_status.flags.mag_3D = false;
 		} 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;
+			float terrain_vpos_estimate = get_terrain_valid() ? _terrain_vpos : _last_on_ground_posD; // WINGTRA
+			bool height_achieved = (terrain_vpos_estimate - _state.pos(2)) > 1.5f; // WINGTRA
 
 			// Check if there has been enough change in horizontal velocity to make yaw observable
 			// Apply hysteresis to check to avoid rapid toggling
@@ -1199,9 +1200,8 @@ void Ekf::controlMagFusion()
 				_time_last_movement = _imu_sample_delayed.time_us;
 			}
 
-			// WINGTRA: Force 3D fusion right after take off
-			if (height_achieved && !_height_achieved_once) {
-				_height_achieved_once = true;
+			// WINGTRA: Force 3D fusion in hover when we are away from the ground
+			if (height_achieved && !_control_status.flags.fixed_wing) {
 				_time_last_movement = _imu_sample_delayed.time_us;
 			}
 			// WINGTA: End
@@ -1212,6 +1212,12 @@ void Ekf::controlMagFusion()
 					(_flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height
 					((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives
 
+			// WINGTRA: Force heading fusion in hover close to the ground
+			if (!_control_status.flags.fixed_wing && !height_achieved) {
+				use_3D_fusion = false;
+			}
+			// WINGTRA: End
+
 			// perform switch-over
 			if (use_3D_fusion) {
 				if (!_control_status.flags.mag_3D) {

EKF/ekf.h

@@ -332,7 +332,6 @@ class Ekf : public EstimatorInterface
 	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
 	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
-	bool _height_achieved_once{false};  ///< WINGTRA: true if the system reached 1.5 meter altitude once
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation moaneouvre was detected
 	uint8_t _num_bad_flight_yaw_events{0};	///< number of times a bad heading has been detected in flight and required a yaw reset
 	bool _mag_yaw_reset_req{false};		///< WINGTRA: true when a reset of the yaw using the magnetomer data has been requested