land_detector: Rework ground effect calculation for MC

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -112,6 +112,12 @@ void MulticopterLandDetector::_update_topics()
 			}
 		}
 	}
+
+	takeoff_status_s takeoff_status;
+
+	if (_takeoff_status_sub.update(&takeoff_status)) {
+		_takeoff_state = takeoff_status.takeoff_state;
+	}
 }
 
 void MulticopterLandDetector::_update_params()
@@ -187,6 +193,12 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		_horizontal_movement = false; // not known
 	}
 
+	if (lpos_available && _vehicle_local_position.dist_bottom_valid) {
+		_below_gnd_effect_hgt = _vehicle_local_position.dist_bottom < _get_gnd_effect_altitude();
+
+	} else {
+		_below_gnd_effect_hgt = false;
+	}
 
 	// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
 	const float thr_pct_hover = hover_thrust_estimate_valid ? 0.6f : 0.3f;
@@ -319,9 +331,22 @@ float MulticopterLandDetector::_get_max_altitude()
 	}
 }
 
+float MulticopterLandDetector::_get_gnd_effect_altitude()
+{
+	if (_param_lndmc_alt_gnd_effect.get() < 0.0f) {
+		return INFINITY;
+
+	} else {
+		return _param_lndmc_alt_gnd_effect.get();
+	}
+}
+
 bool MulticopterLandDetector::_get_ground_effect_state()
 {
-	return _in_descend && !_horizontal_movement;
+
+	return (_in_descend && !_horizontal_movement) ||
+	       (_below_gnd_effect_hgt && _takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT) ||
+	       _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP;
 }
 
 } // namespace land_detector