added ground effect reporting to land detector

Signed-off-by: Roman <bapstroman@gmail.com>

src/modules/ekf2/ekf2_main.cpp

@@ -1250,6 +1250,7 @@ void Ekf2::run()
 		if (vehicle_land_detected_updated) {
 			if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) {
 				_ekf.set_in_air_status(!vehicle_land_detected.landed);
+				//_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
 			}
 		}
 

src/modules/land_detector/LandDetector.cpp

@@ -125,6 +125,8 @@ void LandDetector::_cycle()
 	const bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
 	const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
 
+	const bool in_ground_effect = _ground_effect_hysteresis.get_state();
+
 	const hrt_abstime now = hrt_absolute_time();
 
 	// publish at 1 Hz, very first time, or when the result has changed
@@ -134,6 +136,7 @@ void LandDetector::_cycle()
 	    (_landDetected.freefall != freefallDetected) ||
 	    (_landDetected.maybe_landed != maybe_landedDetected) ||
 	    (_landDetected.ground_contact != ground_contactDetected) ||
+	    (_landDetected.in_ground_effect != in_ground_effect) ||
 	    (fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) {
 
 		if (!landDetected && _landDetected.landed) {
@@ -147,6 +150,7 @@ void LandDetector::_cycle()
 		_landDetected.maybe_landed = maybe_landedDetected;
 		_landDetected.ground_contact = ground_contactDetected;
 		_landDetected.alt_max = alt_max;
+		_landDetected.in_ground_effect = in_ground_effect;
 
 		int instance;
 		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
@@ -207,6 +211,7 @@ void LandDetector::_update_state()
 	_landed_hysteresis.set_state_and_update(_get_landed_state());
 	_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state());
 	_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state());
+	_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state());
 
 	if (_freefall_hysteresis.get_state()) {
 		_state = LandDetectionState::FREEFALL;

src/modules/land_detector/LandDetector.h

@@ -137,6 +137,11 @@ protected:
 	 */
 	virtual float _get_max_altitude() = 0;
 
+	/**
+	 *  @return true if vehicle could be in ground effect (close to ground)
+	 */
+	virtual bool _get_ground_effect_state() { return false; }
+
 	/**
 	 * Convenience function for polling uORB subscriptions.
 	 *
@@ -156,6 +161,7 @@ protected:
 	systemlib::Hysteresis _landed_hysteresis{true};
 	systemlib::Hysteresis _maybe_landed_hysteresis{true};
 	systemlib::Hysteresis _ground_contact_hysteresis{true};
+	systemlib::Hysteresis _ground_effect_hysteresis{false};
 
 	struct actuator_armed_s	_arming {};
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -179,17 +179,17 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	}
 
 	// Check if we are moving horizontally.
-	bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
-					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
+	_horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+				    + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
 
 	// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
 	// we then can assume that the vehicle hit ground
-	bool in_descend = _is_climb_rate_enabled()
-			  && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
-	bool hit_ground = in_descend && !verticalMovement;
+	_in_descend = _is_climb_rate_enabled()
+		      && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
+	bool hit_ground = _in_descend && !verticalMovement;
 
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	if ((_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock())
+	if ((_has_low_thrust() || hit_ground) && (!_horizontalMovement || !_has_position_lock())
 	    && (!verticalMovement || !_has_altitude_lock())) {
 		return true;
 	}
@@ -334,4 +334,14 @@ bool MulticopterLandDetector::_has_minimal_thrust()
 	return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
 }
 
+bool MulticopterLandDetector::_get_ground_effect_state()
+{
+	if (_in_descend && !_horizontalMovement) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
 } // namespace land_detector

src/modules/land_detector/MulticopterLandDetector.h

@@ -76,6 +76,7 @@ protected:
 	bool _get_ground_contact_state() override;
 	bool _get_maybe_landed_state() override;
 	bool _get_freefall_state() override;
+	bool _get_ground_effect_state() override;
 
 	float _get_max_altitude() override;
 private:
@@ -140,6 +141,9 @@ private:
 	hrt_abstime _min_trust_start{0};		///< timestamp when minimum trust was applied first
 	hrt_abstime _landed_time{0};
 
+	bool _in_descend{false};	///< vehicle is desending
+	bool _horizontalMovement{false};	///< vehicle is moving horizontally
+
 	/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
 	float _get_takeoff_throttle();
 	bool _has_altitude_lock();