Update multi-vehicle sync and crab walk docs

2026-06-29 16:00:34 +08:00
parent 8b19a15fbb
commit c093fa32e6
12 changed files with 1355 additions and 102 deletions
@@ -41,6 +41,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    [AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy;
    [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth;

+    // ===== Clumsy 侧脚本/动作驱动的手动等价输入（不走 Medulla IO，不会被 IO 同步覆盖）=====
+    // 手动 IO 字段是 [AsLowerIO]（Medulla→Clumsy，Medulla 每周期回写），Clumsy 侧 MovementTest 写它们会被覆盖。
+    // 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动：
+    // ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转；Vx/Vy/Vth 语义与手动遥控完全一致（m/s、m/s、deg/s）。
+    [FieldMember(desc = "多车联动：脚本驱动使能")] public bool MultiVehicleScriptEnabled;
+    [FieldMember(desc = "多车联动：脚本驱动模式")] public int MultiVehicleScriptMode;
+    [FieldMember(desc = "多车联动：脚本驱动Vx")] public float MultiVehicleScriptVx;
+    [FieldMember(desc = "多车联动：脚本驱动Vy")] public float MultiVehicleScriptVy;
+    [FieldMember(desc = "多车联动：脚本驱动Vth")] public float MultiVehicleScriptVth;
+
    [AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled;
    [FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx;
    [FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh;
@@ -49,10 +59,45 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    public Dictionary<int, VehicleSyncInfo> MultiVehicleFleet = new();
    public VehicleSyncNotification MultiVehicleNotification;

+    // A: 互斥锁固定为独立 readonly 对象，绝不随 MultiVehicleFleet 字段被整体替换而失效。
+    //    所有对 MultiVehicleFleet / _multiVehicleFleetSeen 的读写都必须 lock(FleetLock)。
+    public readonly object FleetLock = new();
+
+    // C: 各 fleet 成员最近一次被 register/notify 刷新的本地时刻（用本地时钟，避免远端时钟偏差）。
+    //    主车 Tick 据此剔除掉线成员；编队就绪要求所有成员新鲜。
+    private readonly Dictionary<int, DateTime> _multiVehicleFleetSeen = new();
+
    [FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
    [FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
    [FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;

+    // G: 车队中心原子快照。三个 float 无法整体原子写，改为整体新建对象后引用赋值（引用赋值原子），
+    //    路径控制器线程只读最近一次完整快照，避免读到 x 新 / y,th 旧的撕裂组合。
+    public sealed class FleetCenterSnapshot
+    {
+        public float X, Y, Th;
+        public long Tick;
+    }
+    private volatile FleetCenterSnapshot _fleetCenter = new();
+    public FleetCenterSnapshot GetFleetCenterSnapshot() => _fleetCenter;
+    private void PublishFleetCenter(float x, float y, float th)
+    {
+        CenterX = x;
+        CenterY = y;
+        CenterTh = th;
+        _fleetCenter = new FleetCenterSnapshot { X = x, Y = y, Th = th, Tick = DateTime.Now.Ticks };
+    }
+
+    // B: 路径控制器最近一次写入自动速度命令的时刻；超时即视为失效（路径结束/早退/卡顿），清零下发。
+    public DateTime MultiVehicleAutoCmdTime = DateTime.MinValue;
+    // D: 路径控制器透传的理想车队中心位姿（世界系），由 idealPos/idealAngle 写入。
+    public bool MultiVehicleAutoHasIdeal;
+    public float MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh;
+
+    // F: notify 序列号（主车单调递增）与从车已应用的最大序列号（丢弃乱序旧包）。
+    private long _multiVehicleNotifySeq;
+    private long _multiVehicleAppliedSeq = -1;
+
    [AsLowerIO(desc = "车号")] public int CarNum = 1;

    #region 夹臂变量
@@ -97,6 +142,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
    // 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
    private float _rotIntegX, _rotIntegY, _rotIntegTh;
+    private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
    private DateTime _rotPiLastTime = DateTime.MinValue;

    // 原地旋转“两车实际 sim 位姿”诊断采样状态（仅主车，通过 Playground WebAPI 读取）。
@@ -211,8 +257,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                var infoJson = Uri.UnescapeDataString(query.Info ?? "");
                var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
                           ?? throw new Exception("VehicleSyncInfo is null");
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
+                {
                    MultiVehicleFleet[query.CarNum] = info;
+                    _multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻
+                }
                return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
            }
            catch (Exception e)
@@ -222,16 +271,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            }
        });

-        PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query =>
+        // F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限；
+        //    用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。
+        PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body =>
        {
            try
            {
-                var notifyJson = Uri.UnescapeDataString(query.Notification ?? "");
-                var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(notifyJson)
+                var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(body ?? "")
                                   ?? throw new Exception("notification is null");
+                // 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
+                // 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
+                if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq &&
+                    notification.Seq > _multiVehicleAppliedSeq - 100)
+                    return JsonConvert.SerializeObject(new { code = 200, message = "stale" });
+                _multiVehicleAppliedSeq = notification.Seq;
+
                MultiVehicleAligned = notification.Aligned;
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
+                {
                    MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
+                    // C: notify 内含整队成员，逐一刷新其本地存活时刻。
+                    var nowSeen = DateTime.Now;
+                    foreach (var key in MultiVehicleFleet.Keys)
+                        _multiVehicleFleetSeen[key] = nowSeen;
+                }
                lock (_multiVehicleNotificationLock)
                {
                    MultiVehicleNotification = notification;
@@ -298,7 +361,14 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    {
        var isMaster = IsMultiVehicleMaster();
        var autoEnabled = false;
-        var manualEnabled = MultiVehicleManualEnabled;
+        // 手动等价输入 = Medulla 遥控(IO) 或 Clumsy 脚本/动作驱动（二者择一，脚本优先）。
+        var scriptOn = MultiVehicleScriptEnabled;
+        var manualEnabled = MultiVehicleManualEnabled || scriptOn;
+        // 主车实际生效的手动模式与速度：脚本使能时取脚本字段，否则取 Medulla 手动 IO 字段。
+        var manualMode = scriptOn ? MultiVehicleScriptMode : MultiVehicleManualMode;
+        var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
+        var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
+        var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;

        if (isMaster)
            autoEnabled = MultiVehicleAutoEnabled;
@@ -324,24 +394,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        {
            _mvDiskLastLog = DateTime.Now;
            int fleetCnt;
-            lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count;
+            lock (FleetLock) fleetCnt = MultiVehicleFleet.Count;
            var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
                             Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
            FleetDiag(
                $"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
                $"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
+                $"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
                $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
                $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
-                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} pos={Conf.PosAvailable}");
+                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}");
        }

        if (!manualEnabled && !autoEnabled)
        {
            UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
            sendMotionPainter.Clear();
-            lock (MultiVehicleFleet)
+            lock (FleetLock)
+            {
                MultiVehicleFleet.Clear();
+                _multiVehicleFleetSeen.Clear();
+            }
            MultiVehicleAutoEnabled = false;
+            MultiVehicleAutoHasIdeal = false;
+            _multiVehicleAppliedSeq = -1;
            _multiVehicleAccumulateTh = 0f;
            _multiVehicleLastThTime = DateTime.Now;

@@ -351,41 +427,53 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            return;
        }

-        if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable())
+        // 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
+        if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
        {
            MultiVehicleAutoEnabled = false;
            Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
            return;
        }

-        var posAvailable = Conf.PosAvailable;
+        // 两类用途解耦（关键语义）：
+        //  - useDetourCorrection (= MultiVehicleSyncUseDetour)：是否用 SLAM 位姿做"车队内姿态纠正"(POS 补偿)。
+        //    false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
+        //  - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
+        //    SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
+        //    纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
+        var autoMode = autoEnabled && !manualEnabled;
+        var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
+        var slamRead = useDetourCorrection || (isMaster && autoMode);
        float selfX = 0, selfY = 0, selfTh = 0;
-        if (posAvailable)
+        if (slamRead)
        {
            var carPos = DetourInterface.getCartLocation();
            selfX = (float)carPos.x;
            selfY = (float)carPos.y;
            selfTh = (float)carPos.th;
        }
+        // fleetPosValid：整队全局姿态是否已知。主车=自身读到 SLAM；从车=主车广播标志（下方覆盖）。
+        var fleetPosValid = slamRead;

        var syncTh = Conf.TestCarSyncTh;
        var syncDistance = Conf.TestCarSyncDistance;
        var deltaDetectCenter = Conf.DeltaDetectCenter;
        float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
        var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
-
-        CenterX = CenterY = CenterTh = 0;
+        float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
+        float crabSteerLimit = 0;
+        var crabReverseEquivalent = false;

        if (isMaster)
        {
-            if (MultiVehicleManualEnabled)
+            if (manualEnabled)
            {
                syncTh = 0;
-                fleetMode = MultiVehicleManualMode;
+                fleetMode = manualMode;
                if (fleetMode == 2)
                {
                    // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
-                    fleetOmega = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
+                    fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
                    fleetVx = 0;
                    fleetFrontTh = 0;
                    fleetRearTh = 0;
@@ -395,13 +483,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                else if (fleetMode == 1)
                {
                    // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
-                    // 舵轮角限制在 ±90°，超出则取反向并令速度取负，避免出现 180° 这类不可达转角。
-                    var vx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
-                    var vy = MultiVehicleManualVy * Conf.ManualCarSyncVyFac;
+                    // 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
+                    // -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
+                    var vx = manualVx * Conf.ManualCarSyncVxFac;
+                    var vy = manualVy * Conf.ManualCarSyncVyFac;
                    var speed = (float)Math.Sqrt(vx * vx + vy * vy);
                    var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
-                    if (crabAngle > 90f) { crabAngle -= 180f; speed = -speed; }
-                    else if (crabAngle < -90f) { crabAngle += 180f; speed = -speed; }
+                    crabInputVx = vx;
+                    crabInputVy = vy;
+                    crabRawAngle = crabAngle;
+                    crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
+
+                    if (crabAngle > crabSteerLimit) { crabAngle -= 180f; speed = -speed; crabReverseEquivalent = true; }
+                    else if (crabAngle < -crabSteerLimit) { crabAngle += 180f; speed = -speed; crabReverseEquivalent = true; }
                    fleetVx = speed;
                    fleetFrontTh = crabAngle;
                    fleetRearTh = crabAngle;
@@ -410,8 +504,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                }
                else
                {
-                    fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
-                    var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
+                    fleetVx = manualVx * Conf.ManualCarSyncVxFac;
+                    var targetTh = manualVth * Conf.ManualCarSyncVthFac;
                    var now = DateTime.Now;
                    var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
                    _multiVehicleLastThTime = now;
@@ -424,9 +518,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            }
            else
            {
-                fleetVx = MultiVehicleAutoVx;
-                fleetFrontTh = MultiVehicleAutoFrontTh;
-                fleetRearTh = MultiVehicleAutoRearTh;
+                // B: 自动模式速度命令新鲜度门控。路径结束/回调早退/卡顿后，控制器不再刷新 AutoCmdTime，
+                //    超时即视为失效：清零下发速度与 idealPos，关闭 AutoEnabled，避免车队按末速度滑行。
+                var autoTimeoutMs = Conf.MultiVehicleAutoCmdTimeoutMs > 0
+                    ? Conf.MultiVehicleAutoCmdTimeoutMs
+                    : Math.Max(200, Conf.MultiVehicleSyncInterval * 4);
+                var autoFresh = (DateTime.Now - MultiVehicleAutoCmdTime).TotalMilliseconds < autoTimeoutMs;
+                if (autoFresh)
+                {
+                    fleetVx = MultiVehicleAutoVx;
+                    fleetFrontTh = MultiVehicleAutoFrontTh;
+                    fleetRearTh = MultiVehicleAutoRearTh;
+                }
+                else
+                {
+                    fleetVx = fleetFrontTh = fleetRearTh = 0;
+                    MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
+                    MultiVehicleAutoHasIdeal = false;
+                    MultiVehicleAutoEnabled = false;
+                    Hedingben.ToastText("自动速度命令超时，已停车（路径结束/控制器停发）", "MultiVehicle-auto-timeout");
+                }
            }
        }
        else if (MultiVehicleNotification != null)
@@ -438,32 +549,42 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            syncTh = notification.SyncTh;
            syncDistance = notification.SyncDistance;
            deltaDetectCenter = notification.DeltaDetectCenter;
-            CenterX = notification.CenterX;
-            CenterY = notification.CenterY;
-            CenterTh = notification.CenterTh;
-            posAvailable = notification.PosAvailable;
+            PublishFleetCenter(notification.CenterX, notification.CenterY, notification.CenterTh);
+            // 从车整队姿态来自主车广播：以广播标志作为 fleetPosValid（自身 slamRead 仅决定是否做本车纠偏）。
+            fleetPosValid = notification.PosAvailable;
            MultiVehicleAutoEnabled = notification.AutoEnabled;
            fleetVx = notification.FleetVx;
            fleetFrontTh = notification.FleetFrontTh;
            fleetRearTh = notification.FleetRearTh;
            fleetMode = notification.Mode;
            fleetOmega = notification.FleetOmega;
+            // D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
+            MultiVehicleAutoHasIdeal = notification.HasIdeal;
+            MultiVehicleAutoIdealX = notification.IdealX;
+            MultiVehicleAutoIdealY = notification.IdealY;
+            MultiVehicleAutoIdealTh = notification.IdealTh;
        }

        var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);

        if (isMaster)
        {
-            lock (MultiVehicleFleet)
-                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
+            lock (FleetLock)
+            {
+                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                    layoutX, layoutY, layoutTh, true);
+                _multiVehicleFleetSeen[CarNum] = DateTime.Now; // 主车自身恒新鲜
+                PruneStaleFleetMembers();                      // C: 剔除掉线从车
+            }

            if (TryInferFleetCenter(out var cx, out var cy, out var cth))
-            {
-                CenterX = cx;
-                CenterY = cy;
-                CenterTh = cth;
-            }
+                PublishFleetCenter(cx, cy, cth);
+
+            // D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
+            //    使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
+            if (!manualEnabled && MultiVehicleAutoEnabled &&
+                Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal)
+                PublishFleetCenter(MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh);
        }

        var selfAligned = !Conf.MultiVehicleUseDetect;
@@ -488,8 +609,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            else selfAligned = false;
        }

-        // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位）
-        if (float.IsNaN(currentSpacing) && posAvailable)
+        // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（需本车已读到全局定位）
+        if (float.IsNaN(currentSpacing) && slamRead)
            currentSpacing = TryGetSlamSpacing(selfX, selfY);

        if (float.IsNaN(currentSpacing))
@@ -499,7 +620,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
                $"MultiVehicle{CarNum}-spacing");

-        lock (MultiVehicleFleet)
+        lock (FleetLock)
        {
            MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
                                  MultiVehicleFleet.Values.All(v => v.Aligned);
@@ -509,9 +630,24 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
        var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
        bool othersDetectOk;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
            othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
        var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
+
+        // H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
+        //    自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
+        //    （用 Detour 时若定位丢失，getCartLocation() 已先行阻塞，此处再兜底要求有效车队中心。）
+        //    手动模式不受限（允许仅靠互识别/遥控行驶）。
+        if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter)
+        {
+            var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
+            if (!fleetCenterValid)
+            {
+                canMove = false;
+                Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
+            }
+        }
+
        if (!canMove)
        {
            fleetVx = 0;
@@ -531,7 +667,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

        var fleetReady = false;
        var fleetCount = 0;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
        {
            fleetCount = MultiVehicleFleet.Count;
            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
@@ -545,7 +681,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        if (fleetReady)
        {
            chassis.SetOriginBias(layoutX, layoutY, layoutTh);
-            chassis.ControlPointRadius = syncDistance / 2f;
+            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
+            var controlRadius = Conf.MultiVehicleControlRadius > 0
+                ? Conf.MultiVehicleControlRadius
+                : syncDistance / 2f;
+            chassis.ControlPointRadius = controlRadius;
+            // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
+            chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;

            if (canMove && Conf.MultiVehicleUseDetect)
            {
@@ -561,7 +703,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
            }

-            if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _))
+            // 车队内姿态纠正(POS 补偿)：仅当 useDetourCorrection 开启且本车读到全局定位时施加。
+            // 这是"定位参与车队内姿态纠正"的唯一开关点——关闭它不影响上面的整队姿态计算/安全门。
+            if (canMove && useDetourCorrection && slamRead && TryInferFleetCenter(out _, out _, out _))
            {
                var supposedPos = LessMath.Transform2D(
                    Tuple.Create(CenterX, CenterY, CenterTh),
@@ -600,24 +744,41 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
                if (canMove && rotating)
                {
+                    // #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
+                    var allowInteg = chassis.LastRotateAligned;
                    var errX = detectDx + posBiasX;    // mm，车体系：本车纵向(前+)应移动量
                    var errY = detectDy + posBiasY;    // mm，车体系：本车横向(左+)应移动量
                    var errTh = detectDth + posBiasTh; // deg，本车应转角
                    rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt);
+                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                    rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt);
+                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                    rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
                        Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
-                        Conf.MultiVehicleRotateCompThMax, dt);
+                        Conf.MultiVehicleRotateCompThMax, dt, allowInteg);
+
+                    // #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
+                    // 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
+                    // 关键：旋转切向也∝转速，故"纠偏:切向"比例全程恒定=匀速段比例（远<1），既杜绝末段轮子乱打方向，
+                    // 又不像按切向钳位那样在匀速段就削弱纠偏。
+                    var absOmega = Math.Abs(fleetOmega);
+                    _rotOmegaPeak = Math.Max(_rotOmegaPeak, absOmega);
+                    if (_rotOmegaPeak > 1e-3f)
+                    {
+                        var compScale = Math.Min(1f, absOmega / _rotOmegaPeak);
+                        rotCompVx *= compScale;
+                        rotCompVy *= compScale;
+                        rotCompOmega *= compScale;
+                    }
                }
                else
                {
-                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时，停止时不再有残留驱动。
+                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时/峰值，停止时不再有残留驱动。
                    _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                    _rotPiLastTime = DateTime.MinValue;
+                    _rotOmegaPeak = 0;
                }

                _mvRotCompVx = rotCompVx;
@@ -642,7 +803,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _rotPoseEpisode = false;
                _rotPosePrevTime = DateTime.MinValue;
                // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
-                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
+                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
@@ -660,11 +821,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            var cx = xDetectCompensate + xPosCompensate;
            var cy = yDetectCompensate + yPosCompensate;
            var cth = thDetectCompensate + thPosCompensate;
+            var crabDbg = isMaster && manualEnabled && fleetMode == 1
+                ? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
+                : "";

            var dbg =
-                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " +
+                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
                $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
+                crabDbg +
                $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
                $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " +
                $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
@@ -690,20 +855,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

        if (isMaster)
        {
-            lock (MultiVehicleFleet)
+            lock (FleetLock)
            {
-                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
+                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                    layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
            }

            if (fleetReady)
            {
                VehicleSyncNotification notification;
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
                {
                    notification = new VehicleSyncNotification
                    {
-                        PosAvailable = posAvailable,
+                        // 广播"整队姿态有效"：主车读到全局定位即为真，从车据此放行自动模式安全门。
+                        PosAvailable = slamRead,
                        CenterX = CenterX,
                        CenterY = CenterY,
                        CenterTh = CenterTh,
@@ -715,10 +881,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                        Mode = fleetMode,
                        FleetOmega = fleetOmega,
                        AutoEnabled = MultiVehicleAutoEnabled,
-                        ManualEnabled = MultiVehicleManualEnabled,
+                        // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
+                        ManualEnabled = manualEnabled,
                        SyncTh = syncTh,
                        SyncDistance = syncDistance,
-                        DeltaDetectCenter = deltaDetectCenter
+                        DeltaDetectCenter = deltaDetectCenter,
+                        // F: 单调递增序列号（从 1 起），从车据此丢弃乱序旧包。
+                        Seq = ++_multiVehicleNotifySeq,
+                        // D: 透传理想车队中心（仅自动模式且控制器给出时有效）。
+                        HasIdeal = !manualEnabled && MultiVehicleAutoEnabled &&
+                                   Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal,
+                        IdealX = MultiVehicleAutoIdealX,
+                        IdealY = MultiVehicleAutoIdealY,
+                        IdealTh = MultiVehicleAutoIdealTh
                    };
                }

@@ -734,16 +909,40 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        }
        else
        {
-            FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh,
+            FireAndForgetRegister(hc, BuildSelfInfo(false, slamRead, selfX, selfY, selfTh,
                layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
        }
    }

    private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";

+    /// <summary>
+    /// C: 剔除超过 TTL 未刷新（register/notify）的 fleet 成员。调用方须已持有 FleetLock。
+    /// 从车崩溃/断网后其条目过期被删除，使 fleetReady(数量==总数) 同时蕴含"全部成员在线且新鲜"，
+    /// 主车不再基于过期 layout/DetectOk 继续 SendMotion+notify。
+    /// </summary>
+    private void PruneStaleFleetMembers()
+    {
+        var ttlMs = Conf.MultiVehicleMemberTtlMs > 0
+            ? Conf.MultiVehicleMemberTtlMs
+            : Math.Max(500, Conf.MultiVehicleSyncInterval * 6);
+        var now = DateTime.Now;
+        var stale = MultiVehicleFleet.Keys
+            .Where(k => k != CarNum &&
+                        (!_multiVehicleFleetSeen.TryGetValue(k, out var seen) ||
+                         (now - seen).TotalMilliseconds > ttlMs))
+            .ToList();
+        foreach (var k in stale)
+        {
+            MultiVehicleFleet.Remove(k);
+            _multiVehicleFleetSeen.Remove(k);
+            Hedingben.ToastText($"剔除掉线成员 car{k}（{ttlMs:F0}ms 未刷新）", $"MultiVehicle{CarNum}-prune");
+        }
+    }
+
    private bool FleetHasPosAvailable()
    {
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
            return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
    }

@@ -751,7 +950,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private float TryGetSlamSpacing(float selfX, float selfY)
    {
        List<VehicleSyncInfo> others;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
            others = MultiVehicleFleet
                .Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
                .Select(kv => kv.Value).ToList();
@@ -764,7 +963,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    {
        centerX = centerY = centerTh = 0;
        List<VehicleSyncInfo> positioned;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
            positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();

        if (positioned.Count == 0) return false;
@@ -791,6 +990,56 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        return ((float)center.Item1, (float)center.Item2, (float)center.Item3);
    }

+    /// <summary>
+    /// 由本车（通常为主车）当前 Detour SLAM 位姿反推车队中心位姿（世界系）。
+    /// 复用 GetLayoutPose + InferFleetCenterFromCar 的同款 SE(2) 反推（center = carWorld ∘ layout⁻¹），
+    /// 保证与自动模式车队中心计算一致。供动作（如 FleetCrabWalk）取路径起点用；无有效定位时返回 false。
+    /// </summary>
+    public bool TryGetFleetCenterFromSlam(out float centerX, out float centerY, out float centerTh)
+    {
+        centerX = centerY = centerTh = 0;
+        var carPos = DetourInterface.getCartLocation();
+        var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
+        var self = new VehicleSyncInfo
+        {
+            X = (float)carPos.x,
+            Y = (float)carPos.y,
+            Th = (float)carPos.th,
+            LayoutX = layoutX,
+            LayoutY = layoutY,
+            LayoutTh = layoutTh
+        };
+        var (cx, cy, cth) = InferFleetCenterFromCar(self);
+        centerX = cx;
+        centerY = cy;
+        centerTh = cth;
+        return true;
+    }
+
+    /// <summary>
+    /// 预热：用主车 SLAM 位姿，立即(1) 对外发布有效"车队中心快照"，(2) 把主车自身以 posAvailable=true
+    /// 写入 fleet 表。供 FleetCrabWalk 等动作在启动控制器 Track() 前调用，解决两个启动期问题：
+    ///  - 控制器首帧通过 MultiVehicleGetFleetPos 读到 (0,0,0) → 误判已到终点、立即结束；
+    ///  - 自动联动循环的安全门 FleetHasPosAvailable（PilotDefinition.cs ~402）在冷启动(无车上报定位)时
+    ///    会把 AutoEnabled 关掉、循环无法发布真实中心。播种主车 posAvailable 条目即可越过该门，使循环正常接管。
+    /// 需在主车上调用；getCartLocation() 无定位时会阻塞（与自动模式一致）。
+    /// </summary>
+    public bool PrimeMasterAutoFromSlam()
+    {
+        var carPos = DetourInterface.getCartLocation();
+        var (lx, ly, lth) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
+        var info = BuildSelfInfo(true, true, (float)carPos.x, (float)carPos.y, (float)carPos.th,
+            lx, ly, lth, false);
+        var (cx, cy, cth) = InferFleetCenterFromCar(info);
+        PublishFleetCenter(cx, cy, cth);
+        lock (FleetLock)
+        {
+            MultiVehicleFleet[CarNum] = info;
+            _multiVehicleFleetSeen[CarNum] = DateTime.Now;
+        }
+        return true;
+    }
+
    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
    {
        // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
@@ -829,7 +1078,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        // 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
        var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
        Dictionary<int, VehicleSyncInfo> fleet;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
            fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);

        var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);
@@ -874,10 +1123,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

    private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
    {
+        // F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。
        var notifyJson = JsonConvert.SerializeObject(notification);
-        var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}";
-        _ = hc.GetStringAsync(url).ContinueWith(t =>
+        var url = $"http://{ip}:{port}/multi-vehicle-notify";
+        var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json");
+        _ = hc.PostAsync(url, content).ContinueWith(t =>
        {
+            content.Dispose();
            if (t.IsFaulted)
                DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
        }, TaskScheduler.Default);
@@ -904,18 +1156,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
    /// </summary>
    private static float RotatePiTerm(float err, ref float integ, float deadband,
-        float pFac, float iFac, float max, float dt)
+        float pFac, float iFac, float max, float dt, bool allowIntegrate = true)
    {
        // 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
        if (Math.Abs(err) < deadband)
            return ClampBias(integ * iFac, max);

-        // 条件积分(抗 windup)：仅当总输出未在同向饱和时才累加误差。
-        // 起步阶段大误差会让 P 项接近/超过 max，此时继续积分会顶满积分器，
-        // 误差反向后需很久才能泄放，造成纠偏"过冲再回拉"。此处饱和即停积分。
+        // 条件积分(抗 windup)：仅当 (a) 允许积分(舵轮已对齐、车在真正转动) 且
+        // (b) 总输出未在同向饱和 时才累加误差。
+        // allowIntegrate=false：上一拍舵轮未对齐(gate=0、车没动)，此时积分误差是纯 windup，
+        // 会让纠偏越积越大、舵轮更对不齐 → 原地卡死；故冻结积分(保留已有值，仅输出 P+已积分)。
+        // 同向饱和停积分：起步大误差让 P 顶满 max 时继续积分会顶满积分器，误差反向后泄放慢、造成过冲回拉。
        var unclamped = err * pFac + integ * iFac;
        var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
-        if (!saturatedSameSign)
+        if (allowIntegrate && !saturatedSameSign)
            integ += err * dt;

        var iTerm = integ * iFac;