added ground effect reporting to land detector

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

msg/vehicle_land_detected.msg

@@ -4,3 +4,4 @@ bool freefall		# true if vehicle is currently in free-fall
 bool ground_contact	# true if vehicle has ground contact but is not landed
 bool maybe_landed	# true if the vehicle might have landed
 float32 alt_max 	# maximum altitude in [m] that can be reached
+bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).

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
+	_horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
-					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
+				    + _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()
+	_in_descend = _is_climb_rate_enabled()
-			  && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
+		      && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
-	bool hit_ground = in_descend && !verticalMovement;
+	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();