diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 0cd3a5d6d19d75c8a867fdb9e915e4fedad0afa1..e3e3a96fcd04ec3de0c3a13d9a2fa86ddde62e57 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -122,6 +122,8 @@ void AC_PosControl::set_accel_z(float accel_cmss)
 void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
 {
     float alt_change = alt_cm-_pos_target.z;
+    
+    _vel_desired.z = constrain_float(alt_change * dt, _speed_down_cms, _speed_up_cms);
 
     // adjust desired alt if motors have not hit their limits
     if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
@@ -144,6 +146,8 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
     if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
         _pos_target.z += climb_rate_cms * dt;
     }
+    
+    _vel_desired.z = climb_rate_cms;
 }
 
 // get_alt_error - returns altitude error in cm