ekf2: consolidate and simplify gnd effect logic

src/modules/ekf2/EKF/control.cpp

@@ -887,7 +887,7 @@ void Ekf::controlHeightFusion()
 
 	updateBaroHgtBias();
 	updateBaroHgtOffset();
-	checkGroundEffectTimeout();
+	updateGroundEffect();
 
 	if (_control_status.flags.baro_hgt) {
 

src/modules/ekf2/EKF/ekf.h

@@ -933,7 +933,7 @@ private:
 	void updateBaroHgtOffset();
 	void updateBaroHgtBias();
 
-	void checkGroundEffectTimeout();
+	void updateGroundEffect();
 
 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1368,14 +1368,23 @@ void Ekf::updateBaroHgtBias()
 	}
 }
 
-void Ekf::checkGroundEffectTimeout()
+void Ekf::updateGroundEffect()
 {
-	// Turn off ground effect compensation if it times out
-	if (_control_status.flags.gnd_effect) {
-		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
+	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
+		if (isTerrainEstimateValid()) {
+			// automatically set ground effect if terrain is valid
+			float height = _terrain_vpos - _state.pos(2);
+			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
-			_control_status.flags.gnd_effect = false;
+		} else if (_control_status.flags.gnd_effect) {
+			// Turn off ground effect compensation if it times out
+			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
+				_control_status.flags.gnd_effect = false;
+			}
 		}
+
+	} else {
+		_control_status.flags.gnd_effect = false;
 	}
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -136,9 +136,9 @@ public:
 	// 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)
+	void set_gnd_effect()
 	{
-		_control_status.flags.gnd_effect = gnd_effect;
+		_control_status.flags.gnd_effect = true;
 		_time_last_gnd_effect_on = _time_last_imu;
 	}
 

src/modules/ekf2/EKF/height_fusion.cpp

@@ -47,10 +47,11 @@ void Ekf::fuseBaroHgt()
 
 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
-	const float deadzone_start = 0.0f;
-	const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+	if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {
+
+		const float deadzone_start = 0.0f;
+		const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
 
-	if (_control_status.flags.gnd_effect) {
 		if (_baro_hgt_innov < -deadzone_start) {
 			if (_baro_hgt_innov <= -deadzone_end) {
 				_baro_hgt_innov += deadzone_end;