land_detector: use rangefinder for ground contact detection

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -83,6 +83,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
 	_paramHandle.altitude_max = param_find("LNDMC_ALT_MAX");
 	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
+	_paramHandle.clearance = param_find("LNDMC_CLEARANCE");
 
 	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
 	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
@@ -101,6 +102,7 @@ void MulticopterLandDetector::_initialize_topics()
 	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+	_distance_sensor_sub = orb_subscribe_multi(ORB_ID(distance_sensor), 0);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -112,6 +114,7 @@ void MulticopterLandDetector::_update_topics()
 	_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
 	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
 	_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
+	_orb_update(ORB_ID(distance_sensor), _distance_sensor_sub, &_distance_sensor);
 }
 
 void MulticopterLandDetector::_update_params()
@@ -129,6 +132,7 @@ void MulticopterLandDetector::_update_params()
 	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
 	param_get(_paramHandle.altitude_max, &_params.altitude_max);
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+	param_get(_paramHandle.clearance, &_params.clearance);
 }
 
 
@@ -159,6 +163,19 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		return true;
 	}
 
+	if (_params.clearance >= 0 &&
+	    hrt_elapsed_time(&_distance_sensor.timestamp) < 200_ms &&
+	    _distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
+	    _distance_sensor.current_distance <= _distance_sensor.max_distance &&
+	    _distance_sensor.current_distance >= _distance_sensor.min_distance)
+	{
+		// rangefinder data is valid
+		if (_distance_sensor.current_distance <= _params.clearance) {
+			return true;
+		}
+		// rangefinder says we're not landed, but we still try another conditions
+	}
+
 	// land speed threshold
 	float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -56,6 +56,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/distance_sensor.h>
 
 using namespace time_literals;
 
@@ -107,6 +108,7 @@ class MulticopterLandDetector : public LandDetector
 		param_t freefall_trigger_time;
 		param_t altitude_max;
 		param_t landSpeed;
+		param_t clearance;
 	} _paramHandle{};
 
 	struct {
@@ -121,6 +123,7 @@ class MulticopterLandDetector : public LandDetector
 		float freefall_trigger_time;
 		float altitude_max;
 		float landSpeed;
+		float clearance;
 	} _params{};
 
 	int _vehicleLocalPositionSub{-1};
@@ -130,6 +133,7 @@ class MulticopterLandDetector : public LandDetector
 	int _sensor_bias_sub{-1};
 	int _vehicle_control_mode_sub{-1};
 	int _battery_sub{-1};
+	int _distance_sensor_sub{-1};
 
 	vehicle_local_position_s				_vehicleLocalPosition {};
 	vehicle_local_position_setpoint_s	_vehicleLocalPositionSetpoint {};
@@ -138,6 +142,7 @@ class MulticopterLandDetector : public LandDetector
 	sensor_bias_s					_sensors {};
 	vehicle_control_mode_s				_control_mode {};
 	battery_status_s						_battery {};
+	distance_sensor_s						_distance_sensor {};
 
 	hrt_abstime _min_trust_start{0};		///< timestamp when minimum trust was applied first
 	hrt_abstime _landed_time{0};

src/modules/land_detector/land_detector_params_mc.c

@@ -129,3 +129,19 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3);
  *
  */
 PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);
+
+/**
+ * Rangefinder clearance for ground contact detection
+ *
+ * Ground contact will be detected, if the rangeinder
+ * distance is less or equal this value.
+ * A negative value indicates no rangefinder usage.
+ *
+ * @unit m
+ * @min -1
+ * @max 10
+ * @decimal 3
+ * @group Land Detector
+ *
+ */
+PARAM_DEFINE_FLOAT(LNDMC_CLEARANCE, 0.02f);