AC_AttitudeControl: AC_PosControl: Prioritize crosstrack acceleration

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