diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
index 00f6186b75..cf5a879d6c 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
@@ -23,14 +23,9 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
             sqrt_controller(_alt_max-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f));
     }
 
-    // calculated increased maximum acceleration if over speed
-    float accel_z_cms = _accel_max_z_cmss;
-    if (_vel_desired.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) {
-        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_down_cms;
-    }
-    if (_vel_desired.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) {
-        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
-    }
+    // calculated increased maximum acceleration and jerk if over speed
+    float accel_max_z_cmss = _accel_max_z_cmss * calculate_overspeed_gain();
+    float jerk_max_xy_cmsss = _jerk_max_xy_cmsss * calculate_overspeed_gain();
 
     // adjust desired alt if motors have not hit their limits
     update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
@@ -42,8 +37,8 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
 
     shape_vel_accel(vel, accel,
         _vel_desired.z, _accel_desired.z,
-        -accel_z_cms, accel_z_cms,
-        _jerk_max_xy_cmsss, _dt, limit_output);
+        -accel_max_z_cmss, accel_max_z_cmss,
+        jerk_max_xy_cmsss, _dt, limit_output);
 
     update_vel_accel(vel, accel, _dt, 0.0, 0.0);
 }