diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 148a4f0b34d5e24248acbc3c9ed209b9b3cacba0..9d168a95cbb08a88a4b05d7113dd2f4edb2cec19 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -392,6 +392,8 @@ private:
 
 	bool manual_wants_takeoff();
 
+	void set_takeoff_velocity(float &vel_sp_z);
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -2515,11 +2517,7 @@ MulticopterPositionControl::calculate_velocity_setpoint()
 
 	/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
 	if (_in_smooth_takeoff) {
-		_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
-		/* ramp vertical velocity limit up to takeoff speed */
-		_takeoff_vel_limit += -_vel_sp(2) * _dt / _takeoff_ramp_time.get();
-		/* limit vertical velocity to the current ramp value */
-		_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
+		set_takeoff_velocity(_vel_sp(2));
 	}
 
 	/* make sure velocity setpoint is constrained in all directions (xyz) */
@@ -3238,6 +3236,16 @@ MulticopterPositionControl::task_main()
 	_control_task = -1;
 }
 
+void
+MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z)
+{
+	_in_smooth_takeoff = _takeoff_vel_limit < -vel_sp_z;
+	/* ramp vertical velocity limit up to takeoff speed */
+	_takeoff_vel_limit += -vel_sp_z * _dt / _takeoff_ramp_time.get();
+	/* limit vertical velocity to the current ramp value */
+	vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit);
+}
+
 int
 MulticopterPositionControl::start()
 {