diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 49695d7cd9..5329ea22cc 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -658,13 +658,13 @@ void AC_PosControl::update_xy_controller() // Acceleration Controller // limit acceleration using maximum lean angles - _limit_vector.x = 0.0f; - _limit_vector.y = 0.0f; float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd()); float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)); - if (_accel_target.limit_length_xy(accel_max)) { - _limit_vector.x = _accel_target.x; - _limit_vector.y = _accel_target.y; + // Define the limit vector before we constrain _accel_target + _limit_vector.xy() = _accel_target.xy(); + if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) { + // _accel_target was not limited so we can zero the xy limit vector + _limit_vector.xy().zero(); } // update angle targets that will be passed to stabilize controller