Update fleet crab walk control

MultiWheel/MultiWheelC/MovementTests.cs

@@ -364,39 +364,32 @@ public class FleetRotateInPlaceTest : MovementTest
 }
 
 // ===== 车队联动-自动蟹行动作 =====
-// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
-// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
 //
-// 手动蟹行已验证丝滑，自动动作只额外做两件事：
-// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
-// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
-//
-// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
 //
 // 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
 public class FleetCrabWalk : MovementDefinition
 {
-    /// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
+    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
     public float CrabAngleDeg = 45f;
 
+    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public float BodyToPathAngleDeg = 45f;
+
     /// <summary>路径长度(mm)。</summary>
     public float CrabLengthMm = 2000f;
 
     /// <summary>行驶速度(m/s)。</summary>
     public float CrabSpeed = 0.2f;
 
-    /// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
+    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
     public float GcpThetaThreshold = 95f;
 
-    /// <summary>横向误差转向增益，沿用 Stanley 形式：atan(gain * lateral / speed)。</summary>
-    public float CorrectionGain = 1f;
-
-    /// <summary>自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大收敛越快。</summary>
-    public float CorrectionAngleThreshold = 8f;
-
-    /// <summary>脚本 Vx/Vy 命令斜率限制(m/s^2)，避免纠偏量变化造成舵角阶跃。</summary>
-    public float CommandAccel = 0.4f;
-
     private bool _stopping;
 
     private void Cleanup()
@@ -420,11 +413,20 @@ public class FleetCrabWalk : MovementDefinition
         Cleanup();
     }
 
-    private static float Slew(float current, float target, float maxStep)
+    private static float Clamp(float value, float min, float max)
     {
-        var diff = target - current;
-        if (Math.Abs(diff) <= maxStep) return target;
-        return current + Math.Sign(diff) * maxStep;
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
     }
 
     public override IEnumerable<bool> Get()
@@ -437,7 +439,8 @@ public class FleetCrabWalk : MovementDefinition
             $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
             $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
             $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
-            $"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}",
+            $"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+            $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
             "FleetCrabDbg");
 
         if (conf.MultiVehicleMasterEndpoint != "/")
@@ -458,25 +461,34 @@ public class FleetCrabWalk : MovementDefinition
         DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
 
         var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
         var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
         var phiRad = phi / 180.0 * Math.PI;
         var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
         var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
 
         DLog.Log(
-            $"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " +
+            $"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
             $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
             "FleetCrabDbg");
 
-        // 手动蟹行已验证丝滑：这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1)，
-        // 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。
-        self.MultiVehicleScriptEnabled = true;
-        self.MultiVehicleScriptMode = 1;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
         self.MultiVehicleScriptVx = 0;
         self.MultiVehicleScriptVy = 0;
         self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoIdealX = x0;
+        self.MultiVehicleAutoIdealY = y0;
+        self.MultiVehicleAutoIdealTh = targetBodyTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
         self.PrimeMasterAutoFromSlam();
-        DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
+        DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
 
         var warmEnd = DateTime.Now.AddSeconds(2.0);
         var warmIter = 0;
@@ -484,8 +496,17 @@ public class FleetCrabWalk : MovementDefinition
         while (!_stopping && DateTime.Now < warmEnd)
         {
             warmIter++;
-            self.MultiVehicleScriptEnabled = true;
-            self.MultiVehicleScriptMode = 1;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = 0;
+            self.MultiVehicleAutoRearTh = 0;
+            self.MultiVehicleAutoIdealX = x0;
+            self.MultiVehicleAutoIdealY = y0;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
             self.PrimeMasterAutoFromSlam();
             var snap = self.GetFleetCenterSnapshot();
             int cnt;
@@ -493,7 +514,7 @@ public class FleetCrabWalk : MovementDefinition
             if (warmIter % 5 == 0)
                 DLog.Log(
                     $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
-                    $"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
                     "FleetCrabDbg");
             if (cnt >= conf.MultiVehicleFleetNum)
             {
@@ -506,27 +527,30 @@ public class FleetCrabWalk : MovementDefinition
             yield return true;
         }
         if (!warmReady)
-            DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
+            DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
                 "FleetCrabDbg");
 
-        Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
+        Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
 
         var iter = 0;
-        var cmdVx = 0f;
-        var cmdVy = 0f;
-        var lastTime = DateTime.Now;
         var lastLog = DateTime.MinValue;
         var finishDistance = Math.Max(20f, conf.FinishDistance);
+        var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
+        var baseSpeed = Math.Abs(CrabSpeed);
+        var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
         var stopReason = "done";
 
         while (!_stopping)
         {
             iter++;
-            var now = DateTime.Now;
-            var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds));
-            lastTime = now;
 
-            self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth);
+            if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
+            {
+                stopReason = "fleet center invalid";
+                DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
+                break;
+            }
             var delta = new Vector2(cx - x0, cy - y0);
             var along = Vector2.Dot(delta, pathDir);
             var lateral = Vector2.Dot(delta, pathLeft);
@@ -534,29 +558,37 @@ public class FleetCrabWalk : MovementDefinition
             if (remain <= finishDistance)
                 break;
 
-            var speed = Math.Abs(CrabSpeed);
-            if (remain < conf.SlowDistance && conf.SlowDistance > 1)
+            var speed = baseSpeed;
+            if (remain < slowDistance)
             {
-                var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow);
-                speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed);
+                var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
+                speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
             }
 
-            var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
-            correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold));
-            var desiredWorldAngle = CommonMath.RoundTh(phi + correction);
-            var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth);
-            var localRad = localAngle / 180.0 * Math.PI;
-            var targetVx = speed * (float)Math.Cos(localRad);
-            var targetVy = speed * (float)Math.Sin(localRad);
-            var maxStep = Math.Max(0.05f, CommandAccel) * dt;
-            cmdVx = Slew(cmdVx, targetVx, maxStep);
-            cmdVy = Slew(cmdVy, targetVy, maxStep);
+            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+            var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
+            var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            biasItem = ClampAbs(biasItem, conf.BiasThreshold);
+            var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
+            var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
+            var idealAlong = Clamp(along, 0f, CrabLengthMm);
+            var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
 
-            self.MultiVehicleScriptEnabled = true;
-            self.MultiVehicleScriptMode = 1;
-            self.MultiVehicleScriptVx = cmdVx;
-            self.MultiVehicleScriptVy = cmdVy;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
             self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = speed;
+            self.MultiVehicleAutoFrontTh = frontTh;
+            self.MultiVehicleAutoRearTh = rearTh;
+            self.MultiVehicleAutoIdealX = ideal.X;
+            self.MultiVehicleAutoIdealY = ideal.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
 
             if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
             {
@@ -566,8 +598,11 @@ public class FleetCrabWalk : MovementDefinition
                 lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
                 DLog.Log(
                     $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
-                    $"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " +
-                    $"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
+                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
+                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
+                    $"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
+                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
                     "FleetCrabDbg");
             }
             yield return true;
@@ -576,14 +611,20 @@ public class FleetCrabWalk : MovementDefinition
         if (_stopping)
             stopReason = "stop";
 
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
         var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
         while (!_stopping && DateTime.Now < settleEnd)
         {
-            self.MultiVehicleScriptEnabled = true;
-            self.MultiVehicleScriptMode = 1;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = 0;
+            self.MultiVehicleAutoRearTh = 0;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
             yield return true;
         }
 
@@ -604,12 +645,10 @@ public class FleetCrabWalkTest : MovementTest
         _proc = new FleetCrabWalk
         {
             CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
+            BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
             CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
             CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
-            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
-            CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
-            CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
-            CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
         };
         _task = new DriveTask(_proc.Get());
         _task.Wait();

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -23,6 +23,8 @@ public class PilotConfig : MultiWheelPilotConfig
     // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
     // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
     [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
+    // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
+    [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
 
     [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
     [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
@@ -96,6 +98,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "Playground WebAPI 基地址")]
     public string PlaygroundWebApiUrl = "http://localhost:18090";
 
+    [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
+    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
+
     [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
     public string PlaygroundRobotName = "agv_multi_1";
 
@@ -149,9 +154,9 @@ public class PilotConfig : MultiWheelPilotConfig
     public bool FleetRotateUseDetourHeading = true;
 
     // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
-    // 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径，
-    // 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
-    [FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")]
+    // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
+    // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
+    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
     public float FleetCrabAngleDeg = 45f;
 
     [FieldMember(desc = "车队蟹行：路径长度(mm)")]
@@ -160,19 +165,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
     public float FleetCrabSpeed = 0.2f;
 
-    // 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。
-    [FieldMember(desc = "车队蟹行：旧几何控制器gcp角度上限(deg)")]
+    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
     public float FleetCrabGcpThetaThreshold = 95f;
 
-    [FieldMember(desc = "车队蟹行：横向误差纠偏增益")]
-    public float FleetCrabCorrectionGain = 1f;
-
-    [FieldMember(desc = "车队蟹行：自动纠偏最大改向角(deg)")]
-    public float FleetCrabCorrectionAngleDeg = 8f;
-
-    [FieldMember(desc = "车队蟹行：脚本速度命令斜率(m/s^2)")]
-    public float FleetCrabCommandAccel = 0.4f;
-
     // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -176,6 +176,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
     private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
     private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;
+    private DateTime _mvRotateWheelOutputLastLog = DateTime.MinValue;
 
     private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
         int manualMode, float manualVx, float manualVy, float manualVth)
@@ -207,6 +208,38 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
     }
 
+    private void LogRotateWheelOutputs(MultiWheelChassis chassis, string phase, float omega, bool force = false)
+    {
+        var now = DateTime.Now;
+        if (!force && (now - _mvRotateWheelOutputLastLog).TotalMilliseconds < 300) return;
+        _mvRotateWheelOutputLastLog = now;
+
+#pragma warning disable CS0612, CS0618
+        var wheels = chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+        var parts = new List<string>();
+        for (var i = 0; i < wheels.Count; i++)
+        {
+            var wheel = wheels[i];
+            if (wheel is DiffSteerWheel diff)
+            {
+                parts.Add(
+                    $"w{i}:tgt={diff.GetSendAngle():0.0} read={diff.ReadAngle():0.0} " +
+                    $"L={diff.GetLeftSendSpeed():0.000} R={diff.GetRightSendSpeed():0.000}");
+            }
+            else
+            {
+                parts.Add(
+                    $"w{i}:tgt={wheel.GetSendAngle():0.0} read={wheel.ReadAngle():0.0} " +
+                    $"v={wheel.GetSendSpeed():0.000}");
+            }
+        }
+
+        DLog.Log(
+            $"car{CarNum} phase={phase} omega={omega:0.000} " + string.Join(" | ", parts),
+            "MultiVehicleRotateWheelOutput");
+    }
+
     private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
     {
         _multiVehicleMotionFeasible = feasible;
@@ -598,7 +631,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 $"| 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} useDetour={Conf.MultiVehicleSyncUseDetour}");
+                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} " +
+                $"useDetour={Conf.MultiVehicleSyncUseDetour} manualDetour={Conf.MultiVehicleManualUseDetourCorrection}");
         }
 
         if (!manualEnabled && !autoEnabled)
@@ -650,9 +684,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         //    false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
         //  - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
         //    SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
-        //    纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
+        //    手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；确需手动 POS 纠偏时再开
+        //    MultiVehicleManualUseDetourCorrection。
         var autoMode = autoEnabled && !manualEnabled;
-        var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
+        var manualDetourCorrection = manualEnabled && Conf.MultiVehicleManualUseDetourCorrection;
+        var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
         var slamRead = useDetourCorrection || (isMaster && autoMode);
         float selfX = 0, selfY = 0, selfTh = 0;
         if (slamRead)
@@ -689,8 +725,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 if (fleetMode == 2)
                 {
                     // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
-                    var rotateInput = scriptOn ? manualVth : ClampFloat(manualVth, -1f, 1f);
-                    fleetOmega = scriptOn ? rotateInput : rotateInput * Conf.ManualCarSyncVthFac;
+                    fleetOmega = manualVth;
                     fleetVx = 0;
                     fleetFrontTh = 0;
                     fleetRearTh = 0;
@@ -701,7 +736,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 {
                     // 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
                     var speed = manualVx * Conf.ManualCarSyncVxFac;
-                    var crabRatio = Math.Max(-60f, Math.Min(60f, manualVy));
+                    var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
                     var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
                     crabInputVx = speed;
                     crabInputVy = crabRatio;
@@ -1126,26 +1161,40 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else if (rotating)
                 {
-                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
-                        rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
-                    if (motionOk && MultiVehicleRotateWheelsReady)
+                    // Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
+                    if (!MultiVehicleRotateWheelsReady)
+                    {
+                        motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                            rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
+                        rotateHoldForAlignment = true;
+                        fleetOmega = 0;
+                        chassis.RampStop();
+                        if (!motionOk || !MultiVehicleRotateWheelsReady)
+                        {
+                            LogMultiVehicleRemoteDecision(
+                                $"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                                $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
+                        }
+                        else
+                        {
+                            LogMultiVehicleRemoteDecision(
+                                $"ROTATE_HOLD_LOCAL_READY master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                                $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
+                        }
+                    }
+                    else
                     {
                         motionOk = chassis.SendRotateMotion(fleetOmega,
                             localCompensateX: rotCompVx, localCompensateY: rotCompVy,
                             localCompensateTh: rotCompOmega);
-                        MultiVehicleRotateWheelsReady = motionOk &&
-                                                        TryCheckRotateWheelAlignment(chassis,
-                                                            Conf.InPlaceRotateWheelAlignDeg,
-                                                            out _multiVehicleRotateAlignDetail);
-                    }
-                    else
-                    {
-                        rotateHoldForAlignment = true;
-                        fleetOmega = 0;
-                        chassis.RampStop();
-                        LogMultiVehicleRemoteDecision(
-                            $"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
-                            $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
+                        var activeAligned = TryCheckRotateWheelAlignment(chassis,
+                            Conf.InPlaceRotateWheelAlignDeg, out _multiVehicleRotateAlignDetail);
+                        if (!motionOk)
+                            MultiVehicleRotateWheelsReady = false;
+                        if (!activeAligned)
+                            LogMultiVehicleRemoteDecision(
+                                $"ROTATE_ACTIVE_ALIGN_WAIT master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                                $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}");
                     }
                 }
                 else
@@ -1165,6 +1214,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     canMove = false;
                     LogMultiVehicleStop(fleetStopReason, true);
                 }
+                LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
+                    fleetOmega);
                 LogMultiVehicleRemoteDecision(
                     $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
@@ -1173,7 +1224,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
 
                 // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
-                if (isMaster)
+                if (isMaster && Conf.MultiVehicleRotatePoseWebApiDiagEnabled)
                     LogRotatePoseSample();
             }
             else
@@ -1653,6 +1704,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     /// </summary>
     private void LogRotatePoseSample()
     {
+        if (!Conf.MultiVehicleRotatePoseWebApiDiagEnabled) return;
+
         var now = DateTime.Now;
         if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
         _rotPoseLastLog = now;