Land detector: revision of the 2 stage landing mechanism

Ground detect: pilot want down or we are on minimum thrust by auto land but no vertical movement
-> Controller should relax x,y corrections and even ramp down desired thrust
Landed: All other conditions are eventually met

src/modules/land_detector/LandDetector.cpp

@@ -56,7 +56,7 @@ LandDetector::LandDetector() :
 	_state{},
 	_freefall_hysteresis(false),
 	_landed_hysteresis(true),
-	_ground_contact_hysteresis(false),
+	_ground_contact_hysteresis(true),
 	_taskShouldExit(false),
 	_taskIsRunning(false),
 	_work{}
@@ -100,8 +100,8 @@ void LandDetector::_cycle()
 	if (!_taskIsRunning) {
 		// Advertise the first land detected uORB.
 		_landDetected.timestamp = hrt_absolute_time();
-		_landDetected.landed = false;
 		_landDetected.freefall = false;
+		_landDetected.landed = false;
 		_landDetected.ground_contact = false;
 
 		// Initialize uORB topics.
@@ -119,19 +119,20 @@ void LandDetector::_cycle()
 
 	_update_state();
 
-	bool landDetected = (_state == LandDetectionState::LANDED);
 	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
-	bool groundContactDetected = (_state == LandDetectionState::GROUND_CONTACT);
+	bool landDetected = (_state == LandDetectionState::LANDED);
+	bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
+
 
 	// Only publish very first time or when the result has changed.
 	if ((_landDetectedPub == nullptr) ||
-	    (_landDetected.landed != landDetected) ||
 	    (_landDetected.freefall != freefallDetected) ||
-	    (_landDetected.ground_contact != groundContactDetected)) {
+	    (_landDetected.landed != landDetected) ||
+	    (_landDetected.ground_contact != ground_contactDetected)) {
 
 		_landDetected.timestamp = hrt_absolute_time();
-		_landDetected.landed = (_state == LandDetectionState::LANDED);
 		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
+		_landDetected.landed = (_state == LandDetectionState::LANDED);
 		_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
 
 		int instance;
@@ -170,21 +171,19 @@ void LandDetector::_update_state()
 {
 	/* ground contact and landed can be true simultaneously but only one state can be true at a particular time
 	 * with higher priority for landed */
-	bool groundContact = _get_ground_contact_state();
-	bool landed = _get_landed_state();
 	bool freefall = _get_freefall_state();
+	bool landed = _get_landed_state();
+	bool groundContact = (landed || _get_ground_contact_state());
 
-	_ground_contact_hysteresis.set_state_and_update(groundContact);
-	_landed_hysteresis.set_state_and_update(landed);
 	_freefall_hysteresis.set_state_and_update(freefall);
+	_landed_hysteresis.set_state_and_update(landed);
+	_ground_contact_hysteresis.set_state_and_update(groundContact);
 
 	if (_freefall_hysteresis.get_state()) {
 		_state = LandDetectionState::FREEFALL;
 
 	} else if (_landed_hysteresis.get_state()) {
 		_state = LandDetectionState::LANDED;
-		/* need to set ground_contact_state to false */
-		_ground_contact_hysteresis.set_state_and_update(false);
 
 	} else if (_ground_contact_hysteresis.get_state()) {
 		_state = LandDetectionState::GROUND_CONTACT;

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -74,7 +74,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
 	_paramHandle.throttleRange = param_find("LNDMC_THR_RANGE");
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
-	_paramHandle.hoverThrottleAuto = param_find("MPC_THR_HOVER");
+	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
 	_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
@@ -111,7 +111,7 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
 	_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
-	param_get(_paramHandle.hoverThrottleAuto, &_params.hoverThrottleAuto);
+	param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
 	param_get(_paramHandle.throttleRange, &_params.throttleRange);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 	param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
@@ -145,57 +145,76 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
 
-	// 10% of throttle range between min and hover
-	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottleAuto - _params.minThrottle) *
-				 _params.throttleRange;
-
-	// Determine the system min throttle based on flight mode
-	if (!_control_mode.flag_control_altitude_enabled) {
-		sys_min_throttle = (_params.minManThrottle + 0.01f);
-	}
-
-	// Check if thrust output is less than the minimum auto throttle param.
-	bool minimalThrust = (_actuators.control[3] <= sys_min_throttle);
-
-	if (minimalThrust && _min_trust_start == 0) {
-		_min_trust_start = now;
-
-
-	} else if (!minimalThrust) {
-		_min_trust_start = 0;
-	}
-
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		_arming_time = 0;
-
 		return true;
 
 	} else if (_arming_time == 0) {
 		_arming_time = now;
 	}
 
-	const bool manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
+	// If in manual flight mode never report landed if the user has more than idle throttle
+	// Check if user commands throttle and if so, report no ground contact based on
+	// the user intent to take off (even if the system might physically still have
+	// ground contact at this point).
+	const bool manual_control_move_down = _get_manual_control_present() && _manual.z < 0.05f;
 
-	// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle
-	if (_state == LandDetectionState::LANDED && manual_control_present && _manual.z < get_takeoff_throttle()) {
+	// Widen acceptance thresholds for landed state right after arming
+	// so that motor spool-up and other effects do not trigger false negatives.
+	float armThresholdFactor = 1.0f;
+
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) {
+		armThresholdFactor = 2.5f;
+	}
+
+	// Check if we are moving vertically - this might see a spike after arming due to
+	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
+	// an accurate in-air indication.
+	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
+
+	// If pilots commands fully down or already below minimal thrust because of auto land and we do not move down we assume ground contact
+	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
+	if ((manual_control_move_down || _get_minimal_thrust()) &&
+	    (!verticalMovement || !_get_position_lock_available())) {
 		return true;
 	}
 
-	// If in manual flight mode never report landed if the user has more than idle throttle
-	// Check if user commands throttle and if so, report not landed based on
-	// the user intent to take off (even if the system might physically still have
-	// ground contact at this point).
-	if (manual_control_present && _manual.z > 0.15f) {
-		return false;
+	return false;
+}
+
+bool MulticopterLandDetector::_get_landed_state()
+{
+	// Time base for this function
+	const uint64_t now = hrt_absolute_time();
+
+	// only trigger flight conditions if we are armed
+	if (!_arming.armed) {
+		return true;
+	}
+
+	// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff
+	if (_state == LandDetectionState::LANDED && _get_manual_control_present()) {
+		if (_manual.z < _get_takeoff_throttle()) {
+			return true;
+
+		} else {
+			// Pilot wants to take off, assume no groundcontact anymore and therefore allow thrust
+			_ground_contact_hysteresis.set_state_and_update(false);
+		}
+	}
+
+	if (_get_minimal_thrust()) {
+		if (_min_trust_start == 0) {
+			_min_trust_start = now;
+		}
+
+	} else {
+		_min_trust_start = 0;
 	}
 
 	// Return status based on armed state and throttle if no position lock is available.
-	if (_vehicleLocalPosition.timestamp == 0 ||
-	    hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
-	    !_vehicleLocalPosition.xy_valid ||
-	    !_vehicleLocalPosition.z_valid) {
-
+	if (!_get_position_lock_available()) {
 		// The system has minimum trust set (manual or in failsafe)
 		// if this persists for 8 seconds AND the drone is not
 		// falling consider it to be landed. This should even sustain
@@ -218,28 +237,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		armThresholdFactor = 2.5f;
 	}
 
-	// Check if we are moving vertically - this might see a spike after arming due to
-	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
-	// an accurate in-air indication.
-	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
-
-	if (!minimalThrust || verticalMovement) {
-		return false;
-	}
-
-	return true;
-}
-
-bool MulticopterLandDetector::_get_landed_state()
-{
-	float armThresholdFactor = 1.0f;
-
-	// Widen acceptance thresholds for landed state right after arming
-	// so that motor spool-up and other effects do not trigger false negatives.
-	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) {
-		armThresholdFactor = 2.5f;
-	}
-
 	// Check if we are moving horizontally.
 	bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
 					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
@@ -251,16 +248,15 @@ bool MulticopterLandDetector::_get_landed_state()
 			(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
 			(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
-
-	if (!_ground_contact_hysteresis.get_state() || rotating || horizontalMovement) {
-		// Sensed movement or thottle high, so reset the land detector.
-		return false;
+	if (_ground_contact_hysteresis.get_state() && _get_minimal_thrust() && !rotating && !horizontalMovement) {
+		// Ground contact, no thrust and no movement -> landed
+		return true;
 	}
 
-	return true;
+	return false;
 }
 
-float MulticopterLandDetector::get_takeoff_throttle()
+float MulticopterLandDetector::_get_takeoff_throttle()
 {
 	/* Position mode */
 	if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
@@ -280,5 +276,32 @@ float MulticopterLandDetector::get_takeoff_throttle()
 	return 0.0f;
 }
 
+bool MulticopterLandDetector::_get_position_lock_available()
+{
+	return !(_vehicleLocalPosition.timestamp == 0 ||
+		 hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
+		 !_vehicleLocalPosition.xy_valid ||
+		 !_vehicleLocalPosition.z_valid);
+}
+
+bool MulticopterLandDetector::_get_manual_control_present()
+{
+	return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
+}
+
+bool MulticopterLandDetector::_get_minimal_thrust()
+{
+	// 10% of throttle range between min and hover
+	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange;
+
+	// Determine the system min throttle based on flight mode
+	if (!_control_mode.flag_control_altitude_enabled) {
+		sys_min_throttle = (_params.minManThrottle + 0.01f);
+	}
+
+	// Check if thrust output is less than the minimum auto throttle param.
+	return _actuators.control[3] <= sys_min_throttle;
+}
+
 
 }

src/modules/land_detector/MulticopterLandDetector.h

@@ -84,7 +84,7 @@ private:
 		param_t maxVelocity;
 		param_t maxRotation;
 		param_t minThrottle;
-		param_t hoverThrottleAuto;
+		param_t hoverThrottle;
 		param_t throttleRange;
 		param_t minManThrottle;
 		param_t freefall_acc_threshold;
@@ -96,7 +96,7 @@ private:
 		float maxVelocity;
 		float maxRotation_rad_s;
 		float minThrottle;
-		float hoverThrottleAuto;
+		float hoverThrottle;
 		float throttleRange;
 		float minManThrottle;
 		float freefall_acc_threshold;
@@ -123,7 +123,10 @@ private:
 	uint64_t _arming_time;
 
 	/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
-	float get_takeoff_throttle();
+	float _get_takeoff_throttle();
+	bool _get_position_lock_available();
+	bool _get_manual_control_present();
+	bool _get_minimal_thrust();
 };