Merge pull request #351 from PX4/ekfGndEffectComp-wip

EKF: Protect against ground effect induced static pressure rise

EKF/common.h

@@ -192,6 +192,9 @@ struct dragSample {
 #define BADACC_PROBATION  (uint64_t)10e6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
 #define BADACC_BIAS_PNOISE	4.9f	///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
 
+// ground effect compensation
+#define GNDEFFECT_TIMEOUT	10E6	///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+
 struct parameters {
 	// measurement source control
 	int32_t fusion_mode{MASK_USE_GPS};		///< bitmasked integer that selects which aiding sources will be used
@@ -235,6 +238,8 @@ struct parameters {
 	float baro_innov_gate{5.0f};		///< barometric and GPS height innovation consistency gate size (STD)
 	float posNE_innov_gate{5.0f};		///< GPS horizontal position innovation consistency gate size (STD)
 	float vel_innov_gate{5.0f};		///< GPS velocity innovation consistency gate size (STD)
+	float gnd_effect_deadzone{5.0f};	///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
+	float gnd_effect_max_hgt{0.5f};		///< Height above ground at which baro ground effect becomes insignificant (m)
 
 	// magnetometer fusion
 	float mag_heading_noise{3.0e-1f};	///< measurement noise used for simple heading fusion (rad)
@@ -416,7 +421,8 @@ union filter_control_status_u {
 		uint32_t update_mag_states_only   : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
 		uint32_t fixed_wing  : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
 		uint32_t mag_fault   : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
-		uint32_t fuse_aspd   : 1; ///< 19 - true when airspedd measurements are being fused
+		uint32_t fuse_aspd   : 1; ///< 19 - true when airspeed measurements are being fused
+		uint32_t gnd_effect  : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
 	} flags;
 	uint32_t value;
 };

EKF/control.cpp

@@ -828,6 +828,17 @@ void Ekf::controlHeightFusion()
 			if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
 				_hgt_sensor_offset = 0.0f;
 			}
+
+			// Turn off ground effect compensation if it times out or sufficient height has been gained
+			// since takeoff.
+			if (_control_status.flags.gnd_effect) {
+				if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT) ||
+						(((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) &&
+						 _control_status.flags.in_air)) {
+					_control_status.flags.gnd_effect = false;
+				}
+			}
+
 		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
 			// switch to gps if there was a reset to gps
 			_fuse_height = true;
@@ -1124,11 +1135,6 @@ void Ekf::controlDragFusion()
 
 void Ekf::controlMagFusion()
 {
-	// If we are using external vision data for heading then no magnetometer fusion is used
-	if (_control_status.flags.ev_yaw) {
-		return;
-	}
-
 	// If we are on ground, store the local position and time to use as a reference
 	// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
 	if (!_control_status.flags.in_air) {
@@ -1138,7 +1144,8 @@ void Ekf::controlMagFusion()
 	}
 
 	// check for new magnetometer data that has fallen behind the fusion time horizon
-	if (_mag_data_ready) {
+	// If we are using external vision data for heading then no magnetometer fusion is used
+	if (!_control_status.flags.ev_yaw && _mag_data_ready) {
 
 		// 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

EKF/estimator_interface.h

@@ -208,6 +208,15 @@ public:
 	// set flag if synthetic sideslip measurement should be fused
 	void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
 
+	// set flag if static pressure rise due to ground effect is expected
+	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
+	// flag will clear after GNDEFFECT_TIMEOUT uSec
+	void set_gnd_effect_flag(bool gnd_effect)
+	{
+		_control_status.flags.gnd_effect = gnd_effect;
+		_time_last_gnd_effect_on = _time_last_imu;
+	}
+
 	// set flag if only only mag states should be updated by the magnetometer
 	void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}
 
@@ -473,6 +482,7 @@ protected:
 	uint64_t _time_last_airspeed{0};	// timestamp of last airspeed measurement in microseconds
 	uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
 	uint64_t _time_last_optflow{0};
+	uint64_t _time_last_gnd_effect_on{0};	//last time the baro ground effect compensation was turned on externally (uSec)
 
 	fault_status_u _fault_status{};
 

EKF/vel_pos_fusion.cpp

@@ -102,6 +102,20 @@ void Ekf::fuseVelPosHeight()
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
 
+			// Compensate for positive static pressure transients (negative vertical position innovations)
+			// casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
+			float deadzone_start = 0.25f * _params.baro_noise;
+			float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+			if (_control_status.flags.gnd_effect) {
+				if (_vel_pos_innov[5] < -deadzone_start) {
+					if (_vel_pos_innov[5] <= -deadzone_end) {
+						_vel_pos_innov[5] += deadzone_end;
+					} else {
+						_vel_pos_innov[5] = -deadzone_start;
+					}
+				}
+			}
+
 		} else if (_control_status.flags.gps_hgt) {
 			fuse_map[5] = true;
 			// vertical position innovation - gps measurement has opposite sign to earth z axis