MulticopterLandDetector: Make land detection time configurable

msg/vehicle_land_detected.msg

@@ -5,3 +5,8 @@ bool ground_contact	# true if vehicle has ground contact but is not landed (1. s
 bool maybe_landed	# true if the vehicle might have landed (2. stage)
 bool landed		# true if vehicle is currently landed on the ground (3. stage)
 bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).
+bool in_descend
+bool has_low_throttle
+bool vertical_movement
+bool horizontal_movement
+bool close_to_ground_or_skipped_check

src/modules/land_detector/AirshipLandDetector.h

@@ -55,7 +55,6 @@ class AirshipLandDetector : public LandDetector
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
 	void _set_hysteresis_factor(const int factor) override {};
-
 };
 
 } // namespace land_detector

src/modules/land_detector/LandDetector.cpp

@@ -100,16 +100,14 @@ void LandDetector::Run()
 		// 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;
+	// Increase land detection time if not close to ground
+	if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) {
+		_set_hysteresis_factor(3);
 
-		} else if (_high_hysteresis_active && _vehicle_local_position.dist_bottom_valid) {
-			_set_hysteresis_factor(1);
-			_high_hysteresis_active = false;
-		}
+	} else {
+		_set_hysteresis_factor(1);
 	}
 
 	const hrt_abstime now_us = hrt_absolute_time();
@@ -147,6 +145,11 @@ void LandDetector::Run()
 		_land_detected.ground_contact = ground_contactDetected;
 		_land_detected.alt_max = alt_max;
 		_land_detected.in_ground_effect = in_ground_effect;
+		_land_detected.in_descend = _get_in_descend();
+		_land_detected.has_low_throttle = _get_has_low_throttle();
+		_land_detected.horizontal_movement = _get_horizontal_movement();
+		_land_detected.vertical_movement = _get_vertical_movement();
+		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.timestamp = hrt_absolute_time();
 		_vehicle_land_detected_pub.publish(_land_detected);
 	}

src/modules/land_detector/LandDetector.h

@@ -136,6 +136,11 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 	 */
 	virtual bool _get_ground_effect_state() { return false; }
 
+	virtual bool _get_in_descend() { return false; }
+	virtual bool _get_has_low_throttle() { return false; }
+	virtual bool _get_horizontal_movement() { return false; }
+	virtual bool _get_vertical_movement() { return false; }
+	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;
 
 	systemlib::Hysteresis _freefall_hysteresis{false};
@@ -168,8 +173,6 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
 	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

@@ -80,9 +80,6 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
-
-	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
-	_set_hysteresis_factor(1);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -162,8 +159,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// land speed threshold, 90% of MPC_LAND_SPEED
 	const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
 
-	bool vertical_movement = true;
-
 	if (lpos_available && _vehicle_local_position.v_z_valid) {
 		// 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
@@ -176,7 +171,10 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
 		}
 
-		vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+		_vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+
+	} else {
+		_vertical_movement = true;
 	}
 
 
@@ -206,22 +204,18 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// 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;
 	const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
-	bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
+	_has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle);
+	bool ground_contact = _has_low_throttle;
 
 	// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
 	// we then can assume that the vehicle hit ground
 	if (_flag_control_climb_rate_enabled) {
-		vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+		vehicle_local_position_setpoint_s trajectory_setpoint;
 
-		if (_vehicle_local_position_setpoint_sub.update(&vehicle_local_position_setpoint)) {
-			// setpoints can briefly be NAN to signal resets, TODO: fix in multicopter position controller
-			const bool descend_vel_sp = PX4_ISFINITE(vehicle_local_position_setpoint.vz)
-						    && (vehicle_local_position_setpoint.vz >= land_speed_threshold);
-
-			const bool descend_acc_sp = PX4_ISFINITE(vehicle_local_position_setpoint.acceleration[2])
-						    && (vehicle_local_position_setpoint.acceleration[2] >= 100.f);
-
-			_in_descend = descend_vel_sp || descend_acc_sp;
+		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+			// Setpoints can be NAN
+			_in_descend = PX4_ISFINITE(trajectory_setpoint.vz)
+				      && (trajectory_setpoint.vz >= land_speed_threshold);
 		}
 
 		// ground contact requires commanded descent until landed
@@ -233,20 +227,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		_in_descend = false;
 	}
 
+	// 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;
+	_close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check;
 
 	// When not armed, consider to have ground-contact
 	if (!_armed) {
 		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 (_is_close_to_ground() || skip_close_to_ground_check) && ground_contact && !_horizontal_movement
-	       && !vertical_movement;
+	return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement
+	       && !_vertical_movement;
 }
 
 bool MulticopterLandDetector::_get_maybe_landed_state()
@@ -368,9 +362,9 @@ bool MulticopterLandDetector::_is_close_to_ground()
 
 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);
+	_ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
+	_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
+	_maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor);
 	_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
 }
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -71,7 +71,11 @@ class MulticopterLandDetector : public LandDetector
 	bool _get_maybe_landed_state() override;
 	bool _get_freefall_state() override;
 	bool _get_ground_effect_state() override;
-
+	bool _get_in_descend() override { return _in_descend; }
+	bool _get_has_low_throttle() override { return _has_low_throttle; }
+	bool _get_horizontal_movement() override { return _horizontal_movement; }
+	bool _get_vertical_movement() override { return _vertical_movement; }
+	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 	float _get_max_altitude() override;
 
 	void _set_hysteresis_factor(const int factor) override;
@@ -83,15 +87,6 @@ class MulticopterLandDetector : public LandDetector
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
 
-	/** Time in us that ground contact condition have to hold before triggering contact ground */
-	static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
-
-	/** Time in us that almost landing conditions have to hold before triggering almost landed . */
-	static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms;
-
-	/** Time in us that landing conditions have to hold before triggering a land. */
-	static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
-
 	/** 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;
 
@@ -117,9 +112,9 @@ class MulticopterLandDetector : public LandDetector
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	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};
@@ -137,10 +132,14 @@ class MulticopterLandDetector : public LandDetector
 
 	bool _in_descend{false};		///< vehicle is desending
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
+	bool _vertical_movement{false};
+	bool _has_low_throttle{false};
+	bool _close_to_ground_or_skipped_check{false};
 	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_TRIG_TIME>)   _param_lndmc_trig_time,
 		(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,

src/modules/land_detector/RoverLandDetector.h

@@ -56,7 +56,6 @@ class RoverLandDetector : public LandDetector
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
 	void _set_hysteresis_factor(const int factor) override {};
-
 };
 
 } // namespace land_detector

src/modules/land_detector/land_detector_params_mc.c

@@ -31,6 +31,22 @@
  *
  ****************************************************************************/
 
+/**
+ * Multicopter land detection trigger time
+ *
+ * Total time it takes to go through all three land detection stages:
+ * ground contact, maybe landed, landed
+ * when all necessary conditions are constantly met.
+ *
+ * @unit s
+ * @min 0.1
+ * @max 10.0
+ * @decimal 1
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
+
 /**
  * Multicopter max climb rate
  *