fligh-mode-manager: First implementation of a three-stage-landing for multirotos, in case LIDAR is available

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 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()) {
+		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));
+		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) = speed;
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 

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

@@ -171,11 +171,14 @@ protected:
 					(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/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.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.15f);
+
 /**
  * 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_CRWL_SPEED", 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
  *