Three stage autonomous multicopter landing

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -227,9 +227,15 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
 
 	// Slow down automatic descend close to ground
-	float land_speed = math::gradual(_dist_to_ground,
-					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					 _param_mpc_land_speed.get(), _param_mpc_z_v_auto_dn.get());
+	float vertical_speed = math::gradual(_dist_to_ground,
+					     _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					     _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
+
+	bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
+
+	if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) {
+		vertical_speed = _param_mpc_land_crawl_speed.get();
+	}
 
 	if (_type_previous != WaypointType::land) {
 		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
@@ -241,7 +247,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	// User input assisted landing
 	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed
-		land_speed *= (1 + _sticks.getPositionExpo()(2));
+		vertical_speed *= (1 + _sticks.getPositionExpo()(2));
 
 		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
 					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
@@ -263,7 +269,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 
 	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
 	_yaw_setpoint = _land_heading;
-	_velocity_setpoint(2) = land_speed;
+	_velocity_setpoint(2) = vertical_speed;
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -171,11 +171,14 @@ class FlightTaskAuto : public FlightTask
 					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
 					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
 					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>)
-					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+					_param_mpc_land_alt1, // altitude at which we start ramping down speed
 					(ParamFloat<px4::params::MPC_LAND_ALT2>)
-					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+					_param_mpc_land_alt2, // altitude at which we descend at land speed
+					(ParamFloat<px4::params::MPC_LAND_ALT3>)
+					_param_mpc_land_alt3, // altitude where we switch to crawl speed, if LIDAR available
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -80,6 +80,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
+	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -119,6 +120,7 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
+	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
 
 	if (_param_lndmc_z_vel_max.get() > _params.landSpeed) {
 		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f",
@@ -157,13 +159,13 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
 
 	// land speed threshold, 90% of MPC_LAND_SPEED
-	const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
+	const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f);
 
 	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
 		// an accurate in-air indication.
-		float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
+		float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
 
 		if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
 			// Widen acceptance thresholds for landed state right after arming
@@ -215,7 +217,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
 			// Setpoints can be NAN
 			_in_descend = PX4_ISFINITE(trajectory_setpoint.vz)
-				      && (trajectory_setpoint.vz >= land_speed_threshold);
+				      && (trajectory_setpoint.vz >= crawl_speed_threshold);
 		}
 
 		// ground contact requires commanded descent until landed

src/modules/land_detector/MulticopterLandDetector.h

@@ -95,6 +95,7 @@ class MulticopterLandDetector : public LandDetector
 		param_t hoverThrottle;
 		param_t minManThrottle;
 		param_t landSpeed;
+		param_t crawlSpeed;
 		param_t useHoverThrustEstimate;
 	} _paramHandle{};
 
@@ -103,6 +104,7 @@ class MulticopterLandDetector : public LandDetector
 		float hoverThrottle;
 		float minManThrottle;
 		float landSpeed;
+		float crawlSpeed;
 		bool useHoverThrustEstimate;
 	} _params{};
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -414,6 +414,16 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
+/**
+ * Land crawl descend rate. Used below
+ *
+ * @unit m/s
+ * @min 0.3
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f);
+
 /**
  * Enable user assisted descent speed for autonomous land routine.
  *
@@ -714,7 +724,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
  * Altitude for 2. step of slow landing (landing)
  *
  * Below this altitude descending velocity gets
- * limited to "MPC_LAND_SPEED".
+ * limited to "MPC_LAND_SPEED"
  * Value needs to be lower than "MPC_LAND_ALT1"
  *
  * @unit m
@@ -725,6 +735,21 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
 
+/**
+ * Altitude for 3. step of slow landing
+ *
+ * Below this altitude descending velocity gets
+ * limited to "MPC_LAND_CRWL", if LIDAR available.
+ * No effect if LIDAR not available
+ *
+ * @unit m
+ * @min 0
+ * @max 122
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.0f);
+
 /**
  * Position control smooth takeoff ramp time constant
  *