From 99619ec9bca7196327ea25dda0a293f87de00db3 Mon Sep 17 00:00:00 2001 From: ChrisRosser <41840611+ChrisRosser@users.noreply.github.com> Date: Mon, 6 Jul 2026 16:52:31 +0100 Subject: [PATCH] AP_Math: fade cross-track accel prioritisation to zero near zero speed limit_accel_xy() splits the acceleration command into along-track and cross-track components using the normalised velocity, and prioritises the cross-track component when the command saturates the maximum lean angle. As the velocity vector rotates through zero (e.g. a hard stick reversal in Loiter, where forward speed passes through zero before reversing) the unit velocity spins rapidly. With any small residual lateral velocity (wind crab, residual velocity from prior manoeuvring) this re-projects the saturated longitudinal braking command onto a rapidly rotating axis and emits a large lateral acceleration spike, seen as a roll wobble at the moment of the stop and as a PSCE.TAE spike at each PSCN.VN zero crossing. Fade the cross-track prioritisation out to a direction-preserving magnitude limit below LIMIT_ACCEL_XY_MIN_SPEED_MS (1 m/s). Above that speed behaviour is unchanged, so path/corner tracking is unaffected. The limiter only acts when the command saturates the lean-angle limit, and stationary hold already used the isotropic branch, so precision position hold is unaffected. Adds a regression test sweeping velocity through zero and asserting no lateral acceleration is injected. Co-Authored-By: Claude Opus 4.8 --- libraries/AP_Math/control.cpp | 74 ++++++++++++++++-------- libraries/AP_Math/control.h | 3 + libraries/AP_Math/tests/test_control.cpp | 26 +++++++++ 3 files changed, 80 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 7039b54e72d..6a410380f83 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -27,6 +27,14 @@ // control default definitions #define CORNER_ACCELERATION_RATIO 1.0/safe_sqrt(2.0) // acceleration reduction to enable zero overshoot corners +// Speed (m/s) below which limit_accel_xy() fades out cross-track prioritisation. +// Near zero speed the direction of travel is ill-defined, so prioritising +// "cross-track" acceleration re-projects a saturated command onto a rapidly +// rotating axis and injects a lateral acceleration spike (e.g. the roll wobble +// seen at the zero-crossing of a hard Loiter stick reversal). Below this speed +// we fade to an isotropic magnitude limit that preserves the commanded direction. +#define LIMIT_ACCEL_XY_MIN_SPEED_MS 1.0f + // Projects velocity forward in time using acceleration, constrained by directional limit. // - If `limit` is non-zero, it defines a direction in which acceleration is constrained. // - The `vel_error` value defines the direction of velocity error (its sign matters, not its magnitude). @@ -442,31 +450,51 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max) if (!is_positive(accel_max)) { return false; } - // limit acceleration to accel_max while prioritizing cross track acceleration - if (accel.length_squared() > sq(accel_max)) { - if (vel.is_zero()) { - // We do not have a direction of travel so do a simple vector length limit - accel.limit_length(accel_max); - } else { - // calculate acceleration in the direction of and perpendicular to the velocity input - const Vector2f vel_unit = vel.normalized(); - // acceleration in the direction of travel - float accel_dir = vel_unit * accel; - // cross track acceleration - Vector2f accel_cross = accel - (vel_unit * accel_dir); - if (accel_cross.limit_length(accel_max)) { - accel_dir = 0.0; - } else { - // limit_length can't absolutely guarantee this subtraction - // won't be slightly negative, so safe_sqrt is used - float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); - accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); - } - accel = accel_cross + vel_unit * accel_dir; - } + // nothing to do unless the acceleration vector exceeds the limit + if (accel.length_squared() <= sq(accel_max)) { + return false; + } + + // isotropic (direction-preserving) magnitude limit. Used directly when there + // is no meaningful direction of travel, and blended in at low speed below. + Vector2f accel_isotropic = accel; + accel_isotropic.limit_length(accel_max); + + const float speed_ms = vel.length(); + if (!is_positive(speed_ms)) { + // We do not have a direction of travel so do a simple vector length limit + accel = accel_isotropic; return true; } - return false; + + // limit acceleration to accel_max while prioritizing cross track acceleration + // calculate acceleration in the direction of and perpendicular to the velocity input + const Vector2f vel_unit = vel / speed_ms; + // acceleration in the direction of travel + float accel_dir = vel_unit * accel; + // cross track acceleration + Vector2f accel_cross = accel - (vel_unit * accel_dir); + if (accel_cross.limit_length(accel_max)) { + accel_dir = 0.0; + } else { + // limit_length can't absolutely guarantee this subtraction + // won't be slightly negative, so safe_sqrt is used + float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); + accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); + } + const Vector2f accel_prioritised = accel_cross + vel_unit * accel_dir; + + // Fade between the isotropic limit (at zero speed) and the cross-track + // prioritised limit (at and above LIMIT_ACCEL_XY_MIN_SPEED_MS). As the + // velocity vector rotates through zero (e.g. a hard stick reversal in + // Loiter), vel_unit spins and the prioritised split would re-project the + // saturated braking command into a lateral acceleration spike. Fading to the + // direction-preserving limit removes that spike. Both blend inputs have + // magnitude <= accel_max, so the result does too. + const float prioritise_ratio = constrain_float(speed_ms / LIMIT_ACCEL_XY_MIN_SPEED_MS, 0.0f, 1.0f); + accel = accel_isotropic * (1.0f - prioritise_ratio) + accel_prioritised * prioritise_ratio; + + return true; } // Limits a 2D acceleration vector with direction-dependent prioritisation. diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 0feefc455b1..1d7e0536e6e 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -134,6 +134,9 @@ void shape_angle_vel_accel(float angle_desired, float angle_vel_desired, float a // - `vel` defines the current direction of motion (used to split the acceleration). // - `accel` is modified in-place to remain within `accel_max`. // - If the full acceleration vector exceeds `accel_max`, it is reshaped to prioritize lateral correction. +// - Near zero speed the cross-track prioritisation is faded out to a simple +// direction-preserving magnitude limit, avoiding a lateral acceleration spike +// as the direction of travel rotates through zero (e.g. a hard stick reversal). // - If `vel` is zero, a simple magnitude limit is applied. // Returns true if the acceleration vector was modified. bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max); diff --git a/libraries/AP_Math/tests/test_control.cpp b/libraries/AP_Math/tests/test_control.cpp index ffabbac054f..f8a596e4195 100644 --- a/libraries/AP_Math/tests/test_control.cpp +++ b/libraries/AP_Math/tests/test_control.cpp @@ -630,5 +630,31 @@ TEST(Control, test_limit_accel) sigaction(SIGFPE, &old_sa_fpe, nullptr); } +TEST(Control, test_limit_accel_reversal_no_lateral_spike) +{ + // A saturated braking command aligned with the North axis must not inject a + // lateral (East) acceleration as the velocity vector rotates through zero + // during a hard reversal (roll wobble at the Loiter stop). A small residual + // lateral velocity is what makes vel_unit rotate through the crossing. + const float accel_max = 5.0f; + // braking command pointing South, saturating the limit + const float accel_cmd_n = -1.5f * accel_max; + const float residual_e = 0.05f; // small lateral residual velocity (m/s) + + float worst_east = 0.0f; + // sweep the North velocity through zero + for (float vn = 2.0f; vn >= -2.0f; vn -= 0.01f) { + const Vector2f vel{vn, residual_e}; + Vector2f accel{accel_cmd_n, 0.0f}; + limit_accel_xy(vel, accel, accel_max); + // command has zero East; output East is pure injected cross-axis error + worst_east = MAX(worst_east, fabsf(accel.y)); + // magnitude must always respect the limit + EXPECT_LE(accel.length(), accel_max * 1.001f); + } + // Without the low-speed fade this reaches ~0.2*accel_max. Assert it stays small. + EXPECT_LT(worst_east, 0.1f * accel_max); +} + AP_GTEST_MAIN() int hal = 0;