mc_pos_control: added gradual landing speed logic

depending on two altitudes that can get set as parameter
the logic linearly slows down from higher land altitude 1 to slower land altitude 2

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -186,6 +186,8 @@ private:
 		param_t z_vel_max_up;
 		param_t z_vel_max_down;
 		param_t z_ff;
+		param_t slow_land_alt1;
+		param_t slow_land_alt2;
 		param_t xy_p;
 		param_t xy_vel_p;
 		param_t xy_vel_i;
@@ -231,6 +233,8 @@ private:
 		float vel_cruise_xy;
 		float vel_max_up;
 		float vel_max_down;
+		float slow_land_alt1;
+		float slow_land_alt2;
 		uint32_t alt_mode;
 
 		int opt_recover;
@@ -313,6 +317,7 @@ private:
 	void		poll_subscriptions();
 
 	static float    throttle_curve(float ctl, float ctr);
+	void _slow_land_gradual_velocity_limit();
 
 	/**
 	 * Update reference for local position projection
@@ -515,6 +520,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
 	_params_handles.xy_vel_cruise	= param_find("MPC_XY_CRUISE");
 	_params_handles.xy_ff		= param_find("MPC_XY_FF");
+	_params_handles.slow_land_alt1  = param_find("MPC_LAND_ALT1");
+	_params_handles.slow_land_alt2  = param_find("MPC_LAND_ALT2");
 	_params_handles.tilt_max_air	= param_find("MPC_TILTMAX_AIR");
 	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
 	_params_handles.tko_speed	= param_find("MPC_TKO_SPEED");
@@ -637,6 +644,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		/* make sure that vel_cruise_xy is always smaller than vel_max */
 		_params.vel_cruise_xy = math::min(_params.vel_cruise_xy, _params.vel_max_xy);
 
+		param_get(_params_handles.slow_land_alt2, &v);
+		_params.slow_land_alt2 = v;
+		param_get(_params_handles.slow_land_alt1, &v);
+		_params.slow_land_alt1 = math::max(v, _params.slow_land_alt2);
+
 		param_get(_params_handles.alt_mode, &v_i);
 		_params.alt_mode = v_i;
 
@@ -929,7 +941,28 @@ MulticopterPositionControl::get_cruising_speed_xy()
 		_pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy);
 }
 
+void
+MulticopterPositionControl::_slow_land_gradual_velocity_limit()
+{
+	/*
+	 * Make sure downward velocity (positive Z) is limited close to ground.
+	 * for now we use the home altitude and assume that we want to land on a similar absolute altitude.
+	 */
+	float altitude_above_home = -_pos(2) + _home_pos.z;
+	float vel_limit = _params.vel_max_down;
+
+	if (altitude_above_home < _params.slow_land_alt2) {
+		vel_limit = _params.land_speed;
 
+	} else if (altitude_above_home < _params.slow_land_alt1) {
+		/* linear function between the two altitudes */
+		float a = (_params.vel_max_down - _params.land_speed) / (_params.slow_land_alt1 - _params.slow_land_alt2);
+		float b = _params.land_speed - a * _params.slow_land_alt2;
+		vel_limit =  a * altitude_above_home + b;
+	}
+
+	_vel_sp(2) = math::min(_vel_sp(2), vel_limit);
+}
 
 void
 MulticopterPositionControl::control_manual(float dt)
@@ -1734,19 +1767,7 @@ MulticopterPositionControl::control_position(float dt)
 		_vel_sp(2) = -1.0f * _params.vel_max_up;
 	}
 
-	/*
+	_slow_land_gradual_velocity_limit();
-	 * Make sure downward velocity (positive Z) is limited close to ground.
-	 * for now we use the home altitude and assume that our Z coordinate
-	 * is initialized close to home.
-	 */
-	bool close_to_ground = (-_pos(2) + _home_pos.z)  < _manual_land_alt.get();
-
-	if (close_to_ground && (_vel_sp(2) > _params.land_speed)) {
-		_vel_sp(2) = _params.land_speed;
-
-	} else if (_vel_sp(2) >  _params.vel_max_down) {
-		_vel_sp(2) = _params.vel_max_down;
-	}
 
 	if (!_control_mode.flag_control_position_enabled) {
 		_reset_pos_sp = true;

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -512,3 +512,33 @@ PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.0f);
+
+/**
+ * Altitude for 1. step of slow landing (descend)
+ *
+ * Below this altitude descending velocity gets limited
+ * to a value between "MPC_Z_VEL_MAX" and "MPC_LAND_SPEED"
+ * to enable a smooth descent experience
+ * Value needs to be higher than "MPC_LAND_ALT2"
+ *
+ * @unit m
+ * @min 0
+ * @max 122
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+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"
+ * Value needs to be lower than "MPC_LAND_ALT1"
+ *
+ * @unit m
+ * @min 0
+ * @max 122
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);