Update fleet crab walk control

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;