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

src/modules/land_detector/MulticopterLandDetector.h

@@ -47,6 +47,7 @@
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/takeoff_status.h>
 
 #include "LandDetector.h"
 
@@ -74,6 +75,8 @@ protected:
 	float _get_max_altitude() override;
 private:
 
+	float _get_gnd_effect_altitude();
+
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
 
@@ -111,6 +114,7 @@ private:
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
 
 	hrt_abstime _hover_thrust_estimate_last_valid{0};
 
@@ -119,18 +123,22 @@ private:
 
 	float _actuator_controls_throttle{0.f};
 
+	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
+
 	hrt_abstime _min_thrust_start{0};	///< timestamp when minimum trust was applied first
 	hrt_abstime _landed_time{0};
 
 	bool _in_descend{false};		///< vehicle is desending
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
+	bool _below_gnd_effect_hgt{false};	///< vehicle height above ground is below height where ground effect occurs
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(
 		LandDetector,
 		(ParamFloat<px4::params::LNDMC_ALT_MAX>)    _param_lndmc_alt_max,
 		(ParamFloat<px4::params::LNDMC_ROT_MAX>)    _param_lndmc_rot_max,
 		(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
-		(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>)  _param_lndmc_z_vel_max
+		(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>)  _param_lndmc_z_vel_max,
+		(ParamFloat<px4::params::LNDMC_ALT_GND>)    _param_lndmc_alt_gnd_effect
 	);
 };
 

src/modules/land_detector/land_detector_params_mc.c

@@ -84,3 +84,17 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
  *
  */
 PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);
+
+/**
+ * Ground effect altitude for multicopters
+ *
+ * The height above ground below which ground effect creates barometric altitude errors.
+ * A negative value indicates no ground effect.
+ *
+ * @unit m
+ * @min -1
+ * @decimal 2
+ * @group Land Detector
+ *
+ */
+PARAM_DEFINE_FLOAT(LNDMC_ALT_GND, -1.0f);