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;