From fcf4b8ad5841805b7c413d4ab18d7af3c80b6bfa Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 28 Jan 2022 06:08:24 +1030 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Prioritize crosstrack acceleration --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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