diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 7a14bd92c5..fa7fbd6978 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -378,7 +378,7 @@ void AC_PosControl::pos_to_rate_z() } // calculate _vel_target.z using from _pos_error.z using sqrt controller - _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms); + _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt); // check speed limits // To-Do: check these speed limits here or in the pos->rate controller