Fix fleet in-place rotation coordination

- Gate rotate-mode compensation PI by active fleet omega and reset the
  integrator/timer when idle, so releasing the joystick stops the cars
  cleanly instead of drifting/self-rotating from a wound-up integral.
- Add conditional integration anti-windup in RotatePiTerm to reduce the
  startup overshoot after the initial relative-pose spike.
- Raise translational P gain for faster startup correction.
- Superimpose body-frame compensation onto swerve rotation and add
  MultiVehicleDbg / RotateDbg / RotatePoseDbg diagnostics.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
2026-06-28 19:09:24 +08:00
co-authored by Cursor
parent 235c6c70ac
commit d3abf0fb02
4 changed files with 387 additions and 112 deletions
@@ -32,6 +32,10 @@ public class VehicleSyncNotification
[JsonProperty("FleetVx")] public float FleetVx { get; set; }
[JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
[JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
// 联动运动模式:0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
[JsonProperty("Mode")] public int Mode { get; set; }
// 原地旋转角速度(deg/s,逆时针为正),仅 Mode==2 有效
[JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
[JsonProperty("SyncTh")] public float SyncTh { get; set; }