land_detector: robustify land detection by using distance to ground info

- if distance to the ground is available then hysteresis times will be increased
by a factor of 3 if vehicle is higher than 1m above ground

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

src/modules/land_detector/AirshipLandDetector.h

@@ -54,6 +54,7 @@ public:
 protected:
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
+	void _set_hysteresis_factor(const int factor) override {};
 
 };
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -61,6 +61,7 @@ public:
 protected:
 
 	bool _get_landed_state() override;
+	void _set_hysteresis_factor(const int factor) override {};
 
 private:
 

src/modules/land_detector/LandDetector.cpp

@@ -96,6 +96,22 @@ void LandDetector::Run()
 
 	_update_topics();
 
+	if (!_dist_bottom_is_observable) {
+		// we consider the distance to the ground observable if the system is using a range sensor
+		_dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield &
+					     vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE;
+
+	} else {
+		if (!_high_hysteresis_active && !_vehicle_local_position.dist_bottom_valid) {
+			_set_hysteresis_factor(3);
+			_high_hysteresis_active = true;
+
+		} else if (_high_hysteresis_active && _vehicle_local_position.dist_bottom_valid) {
+			_set_hysteresis_factor(1);
+			_high_hysteresis_active = false;
+		}
+	}
+
 	const hrt_abstime now_us = hrt_absolute_time();
 
 	_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);

src/modules/land_detector/LandDetector.h

@@ -136,6 +136,8 @@ protected:
 	 */
 	virtual bool _get_ground_effect_state() { return false; }
 
+	virtual void _set_hysteresis_factor(const int factor) = 0;
+
 	systemlib::Hysteresis _freefall_hysteresis{false};
 	systemlib::Hysteresis _landed_hysteresis{true};
 	systemlib::Hysteresis _maybe_landed_hysteresis{true};
@@ -149,6 +151,7 @@ protected:
 
 	bool _armed{false};
 	bool _previous_armed_state{false};	///< stores the previous actuator_armed.armed state
+	bool _dist_bottom_is_observable{false};
 
 private:
 	void Run() override;
@@ -165,6 +168,8 @@ private:
 	hrt_abstime _takeoff_time{0};
 	hrt_abstime _total_flight_time{0};	///< total vehicle flight time in microseconds
 
+	bool _high_hysteresis_active{false};
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -82,10 +82,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
 
 	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
-	_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
-	_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
-	_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
-	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
+	_set_hysteresis_factor(1);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -242,9 +239,14 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		return true;
 	}
 
+	// if there is no distance to ground estimate available then don't enforce using it.
+	// if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then
+	// we already increased the hysteresis for the land detection states in order to reduce the chance of false positives.
+	const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
 
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	return ground_contact && !_horizontal_movement && !vertical_movement;
+	return (_is_close_to_ground() || skip_close_to_ground_check) && ground_contact && !_horizontal_movement
+	       && !vertical_movement;
 }
 
 bool MulticopterLandDetector::_get_maybe_landed_state()
@@ -354,4 +356,22 @@ bool MulticopterLandDetector::_get_ground_effect_state()
 	       _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP;
 }
 
+bool MulticopterLandDetector::_is_close_to_ground()
+{
+	if (_vehicle_local_position.dist_bottom_valid) {
+		return _vehicle_local_position.dist_bottom < DIST_FROM_GROUND_THRESHOLD;
+
+	} else {
+		return false;
+	}
+}
+
+void MulticopterLandDetector::_set_hysteresis_factor(const int factor)
+{
+	_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US * factor);
+	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US * factor);
+	_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US * factor);
+	_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
+}
+
 } // namespace land_detector

src/modules/land_detector/MulticopterLandDetector.h

@@ -73,9 +73,12 @@ protected:
 	bool _get_ground_effect_state() override;
 
 	float _get_max_altitude() override;
+
+	void _set_hysteresis_factor(const int factor) override;
 private:
 
 	float _get_gnd_effect_altitude();
+	bool _is_close_to_ground();
 
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
@@ -92,6 +95,9 @@ private:
 	/** Time interval in us in which wider acceptance thresholds are used after landed. */
 	static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
 
+	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
+	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;
+
 	/** Handles for interesting parameters. **/
 	struct {
 		param_t minThrottle;

src/modules/land_detector/RoverLandDetector.h

@@ -55,6 +55,7 @@ public:
 protected:
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
+	void _set_hysteresis_factor(const int factor) override {};
 
 };