ManualPosition: Limit horizontal speed during landing

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -113,6 +113,8 @@ void FlightTaskManualPosition::_scaleSticks()
 		}
 	}
 
+	_velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale);
+
 	// scale velocity to its maximum limits
 	Vector2f vel_sp_xy = stick_xy * _velocity_scale;
 
@@ -129,6 +131,20 @@ void FlightTaskManualPosition::_scaleSticks()
 	_velocity_setpoint(1) = vel_sp_xy(1);
 }
 
+float FlightTaskManualPosition::_computeVelXYGroundDist()
+{
+	float max_vel_xy = _constraints.speed_xy;
+
+	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(_dist_to_ground)) {
+		max_vel_xy = math::gradual(_dist_to_ground,
+					   _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					   _param_mpc_land_vel_xy.get(), _constraints.speed_xy);
+	}
+
+	return max_vel_xy;
+}
+
 void FlightTaskManualPosition::_updateXYlock()
 {
 	/* If position lock is not active, position setpoint is set to NAN.*/

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -65,11 +65,13 @@ protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_LAND_VEL_XY>) _param_mpc_land_vel_xy,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_ACC_HOR_ESTM>) _param_mpc_acc_hor_estm
 				       )
 private:
+	float _computeVelXYGroundDist();
 	float _velocity_scale{0.0f}; //scales the stick input to velocity
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */