Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: fade cross-track accel prioritisation to zero near zero speed

Browse Source 
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 <noreply@anthropic.com>
This commit is contained in:
ChrisRosser
2026-07-27 12:10:17 +09:00
committed by Randy Mackay
co-authored by Claude Opus 4.8
parent 1f6e646d2a
commit 99619ec9bc
3 changed files with 80 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Math/control.cpp
+51 -23
View File
@@ -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.