landdetector: add additional landdetection state

msg/vehicle_land_detected.msg

@@ -1,4 +1,5 @@
 bool landed		# true if vehicle is currently landed on the ground
 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

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -112,6 +112,13 @@ bool FixedwingLandDetector::_get_ground_contact_state()
 	return false;
 }
 
+bool FixedwingLandDetector::_get_maybe_landed_state()
+{
+
+	// TODO
+	return false;
+}
+
 bool FixedwingLandDetector::_get_landed_state()
 {
 	// only trigger flight conditions if we are armed

src/modules/land_detector/FixedwingLandDetector.h

@@ -65,6 +65,8 @@ protected:
 
 	virtual bool _get_landed_state() override;
 
+	virtual bool _get_maybe_landed_state() override;
+
 	virtual bool _get_ground_contact_state() override;
 
 	virtual bool _get_freefall_state() override;

src/modules/land_detector/LandDetector.cpp

@@ -57,6 +57,7 @@ LandDetector::LandDetector() :
 	_state{},
 	_freefall_hysteresis(false),
 	_landed_hysteresis(true),
+	_maybe_landed_hysteresis(true),
 	_ground_contact_hysteresis(true),
 	_total_flight_time{0},
 	_takeoff_time{0},
@@ -64,6 +65,7 @@ LandDetector::LandDetector() :
 {
 	// 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);
+	_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
 	_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
 }
 
@@ -93,6 +95,7 @@ void LandDetector::_cycle()
 		_landDetected.freefall = false;
 		_landDetected.landed = false;
 		_landDetected.ground_contact = false;
+		_landDetected.maybe_landed = false;
 		_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
 		_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
 
@@ -117,6 +120,7 @@ void LandDetector::_cycle()
 
 	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
 	bool landDetected = (_state == LandDetectionState::LANDED);
+	bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED);
 	bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
 
 	// Only publish very first time or when the result has changed.
@@ -124,6 +128,7 @@ void LandDetector::_cycle()
 	    (_landDetected.freefall != freefallDetected) ||
 	    (_landDetected.landed != landDetected) ||
 	    (_landDetected.ground_contact != ground_contactDetected) ||
+	    (_landDetected.maybe_landed != maybe_landedDetected) ||
 	    (fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) {
 
 		if (!landDetected && _landDetected.landed) {
@@ -144,6 +149,7 @@ void LandDetector::_cycle()
 		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
 		_landDetected.landed = (_state == LandDetectionState::LANDED);
 		_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
+		_landDetected.maybe_landed = (_state == LandDetectionState::MAYBE_LANDED);
 		_landDetected.alt_max = _altitude_max;
 
 		int instance;
@@ -188,6 +194,7 @@ void LandDetector::_update_state()
 	 * with higher priority for landed */
 	_freefall_hysteresis.set_state_and_update(_get_freefall_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(_landed_hysteresis.get_state() || _get_ground_contact_state());
 
 	if (_freefall_hysteresis.get_state()) {
@@ -196,6 +203,9 @@ void LandDetector::_update_state()
 	} else if (_landed_hysteresis.get_state()) {
 		_state = LandDetectionState::LANDED;
 
+	} else if (_maybe_landed_hysteresis.get_state()) {
+		_state = LandDetectionState::MAYBE_LANDED;
+
 	} else if (_ground_contact_hysteresis.get_state()) {
 		_state = LandDetectionState::GROUND_CONTACT;
 

src/modules/land_detector/LandDetector.h

@@ -60,7 +60,8 @@ public:
 		FLYING = 0,
 		LANDED = 1,
 		FREEFALL = 2,
-		GROUND_CONTACT = 3
+		GROUND_CONTACT = 3,
+		MAYBE_LANDED = 4
 	};
 
 	LandDetector();
@@ -115,6 +116,11 @@ protected:
 	 */
 	virtual bool _get_landed_state() = 0;
 
+	/**
+	 * @return true if UAV is in almost landed state
+	 */
+	virtual bool _get_maybe_landed_state() = 0;
+
 	/**
 	 * @return true if UAV is touching ground but not landed
 	 */
@@ -141,7 +147,10 @@ protected:
 	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
 
 	/** Time in us that landing conditions have to hold before triggering a land. */
-	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 1500000;
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 500000;
+
+	/** Time in us that almost landing conditions have to hold before triggering almost landed . */
+	static constexpr uint64_t MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 1500000;
 
 	/** Time in us that ground contact condition have to hold before triggering contact ground */
 	static constexpr uint64_t GROUND_CONTACT_TRIGGER_TIME_US = 1000000;
@@ -158,6 +167,7 @@ protected:
 
 	systemlib::Hysteresis _freefall_hysteresis;
 	systemlib::Hysteresis _landed_hysteresis;
+	systemlib::Hysteresis _maybe_landed_hysteresis;
 	systemlib::Hysteresis _ground_contact_hysteresis;
 
 	float _altitude_max;

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -186,10 +186,15 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 
 	// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	return manual_control_idle_or_auto && _has_minimal_thrust() && (!verticalMovement || !_has_altitude_lock());
+	if (manual_control_idle_or_auto && _has_low_thrust() &&
+	    (!verticalMovement || !_has_altitude_lock())) {
+		return true;
+	}
+
+	return false;
 }
 
-bool MulticopterLandDetector::_get_landed_state()
+bool MulticopterLandDetector::_get_maybe_landed_state()
 {
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
@@ -257,6 +262,17 @@ bool MulticopterLandDetector::_get_landed_state()
 	return false;
 }
 
+bool MulticopterLandDetector::_get_landed_state()
+{
+	// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
+	// therefore check if all other condition of the landed state remain true
+	if (_maybe_landed_hysteresis.get_state()) {
+		return true;
+	}
+
+	return false;
+}
+
 float MulticopterLandDetector::_get_takeoff_throttle()
 {
 	/* Position mode */
@@ -314,9 +330,21 @@ bool MulticopterLandDetector::_has_manual_control_present()
 	return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
 }
 
+bool MulticopterLandDetector::_has_low_thrust()
+{
+	// 30% of throttle range between min and hover
+	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f;
+
+	PX4_INFO("_actuatl control 3: %.5f, sys_min_throttle: %.5f", (double)_actuators.control[3], (double)sys_min_throttle);
+
+	// Check if thrust output is less than the minimum auto throttle param.
+	return _actuators.control[3] <= sys_min_throttle;
+
+}
+
 bool MulticopterLandDetector::_has_minimal_thrust()
 {
-	// 10% of throttle range between min and hover
+	// 10% of throttle range between min and hover once we entered ground contact
 	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange;
 
 	// Determine the system min throttle based on flight mode
@@ -328,5 +356,4 @@ bool MulticopterLandDetector::_has_minimal_thrust()
 	return _actuators.control[3] <= sys_min_throttle;
 }
 
-
 }

src/modules/land_detector/MulticopterLandDetector.h

@@ -74,6 +74,8 @@ protected:
 
 	virtual bool _get_ground_contact_state() override;
 
+	virtual bool _get_maybe_landed_state() override;
+
 	virtual bool _get_freefall_state() override;
 
 	virtual float _get_max_altitude() override;
@@ -139,6 +141,7 @@ private:
 	bool _has_position_lock();
 	bool _has_manual_control_present();
 	bool _has_minimal_thrust();
+	bool _has_low_thrust();
 };
 
 

src/modules/land_detector/VtolLandDetector.cpp

@@ -76,6 +76,13 @@ bool VtolLandDetector::_get_ground_contact_state()
 	return MulticopterLandDetector::_get_ground_contact_state();
 }
 
+bool VtolLandDetector::_get_maybe_landed_state()
+{
+
+	// TODO
+	return false;
+}
+
 bool VtolLandDetector::_get_landed_state()
 {
 	// this is returned from the mutlicopter land detector

src/modules/land_detector/VtolLandDetector.h

@@ -62,6 +62,8 @@ protected:
 
 	virtual bool _get_landed_state() override;
 
+	virtual bool _get_maybe_landed_state() override;
+
 	virtual bool  _get_ground_contact_state() override;
 
 	virtual bool _get_freefall_state() override;