Fix fleet in-place rotate sync

MultiWheel/MultiWheelC/MovementTests.cs

@@ -206,7 +206,11 @@ public class FleetRotateInPlace : MovementDefinition
         var start = DateTime.Now;
         var lastTime = start;
         var lastLog = DateTime.MinValue;
+        var lastCenterLog = DateTime.MinValue;
         var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
+        var centerTracking = false;
+        float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
+        float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
         // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
         var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
         if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
@@ -239,20 +243,43 @@ public class FleetRotateInPlace : MovementDefinition
             yield return true;
         }
 
+        float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
         if (hasPos)
         {
-            prevTh = (float)DetourInterface.getCartLocation().th;
+            var startPos = DetourInterface.getCartLocation();
+            centerStartCarX = (float)startPos.x;
+            centerStartCarY = (float)startPos.y;
+            centerStartCarTh = (float)startPos.th;
+            prevTh = centerStartCarTh;
             startTh = prevTh;
+            if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
+                    out centerStartX, out centerStartY, out centerStartTh))
+            {
+                centerLastX = centerStartX;
+                centerLastY = centerStartY;
+                centerLastTh = centerStartTh;
+                centerMaxDrift = 0;
+                centerTracking = true;
+            }
         }
         accumulated = 0f;
         start = DateTime.Now;
         lastTime = start;
         lastLog = DateTime.MinValue;
+        lastCenterLog = DateTime.MinValue;
         cmdMag = 0f;
         DLog.Log(
             $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
             $"fleetAligned={self.MultiVehicleRotateFleetReady}",
             "FleetRotateDbg");
+        if (centerTracking)
+        {
+            DLog.Log(
+                $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
+                $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
+                "FleetRotateCenterDbg");
+        }
 
         var stopReason = "stop()";
         while (true)
@@ -266,7 +293,8 @@ public class FleetRotateInPlace : MovementDefinition
             float curTh = 0f, remaining = 0f, actualRate = 0f;
             if (hasPos)
             {
-                curTh = (float)DetourInterface.getCartLocation().th;
+                var carPos = DetourInterface.getCartLocation();
+                curTh = (float)carPos.th;
                 var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
                 accumulated += step;
                 actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
@@ -280,6 +308,28 @@ public class FleetRotateInPlace : MovementDefinition
                 desiredMag = remaining < slowDeg
                     ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
                     : maxOmega;
+
+                if (centerTracking &&
+                    self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
+                        out centerLastX, out centerLastY, out centerLastTh))
+                {
+                    var centerDx = centerLastX - centerStartX;
+                    var centerDy = centerLastY - centerStartY;
+                    var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+                    centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
+                    var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+                    if ((now - lastCenterLog).TotalMilliseconds >= 250)
+                    {
+                        lastCenterLog = now;
+                        DLog.Log(
+                            $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                            $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                            $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                            $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
+                            $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
+                            "FleetRotateCenterDbg");
+                    }
+                }
             }
             else
             {
@@ -329,6 +379,19 @@ public class FleetRotateInPlace : MovementDefinition
             $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
             $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
             "FleetRotateDbg");
+        if (centerTracking)
+        {
+            var centerDx = centerLastX - centerStartX;
+            var centerDy = centerLastY - centerStartY;
+            var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+            var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+            DLog.Log(
+                $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
+                "FleetRotateCenterDbg");
+        }
         Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
     }
 }

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -87,10 +87,9 @@ public class PilotConfig : MultiWheelPilotConfig
     // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
     // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
     [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
-    // 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
+    // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
-    // 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。
+    // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
-    // 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。
+    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
-    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f;
 
     [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
     [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
@@ -125,6 +124,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
     public float InPlaceRotateWheelAlignDeg = 2f;
 
+    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
+    public float InPlaceRotateActiveWheelAlignDeg = 10f;
+
     // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
     // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
     [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -146,12 +146,31 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private float _multiVehicleRotateDirectionHint = 1f;
     private string _multiVehicleRotateAlignDetail = "";
     private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;
+    private const float DefaultRotateCompTangentFrac = 0.10f;
+
+    private struct RotateControlParams
+    {
+        public float ActiveOmega;
+        public float CompXyFac;
+        public float CompXyIFac;
+        public float CompXyMax;
+        public float CompThFac;
+        public float CompThIFac;
+        public float CompThMax;
+        public float CompTangentFrac;
+        public float StartWheelAlignDeg;
+        public float ActiveWheelAlignDeg;
+    }
 
     // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
     private DateTime _mvDbgLastLog = DateTime.MinValue;
     private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
     // 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
     private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
+    private DateTime _mvRotateCompLimitLastLog = DateTime.MinValue;
+    private bool _mvRotateCenterDriftActive;
+    private float _mvRotateCenterStartX, _mvRotateCenterStartY, _mvRotateCenterStartTh, _mvRotateCenterMaxDrift;
+    private DateTime _mvRotateCenterLastLog = DateTime.MinValue;
     // 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
     private float _rotIntegX, _rotIntegY, _rotIntegTh;
     private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
@@ -253,7 +272,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return value;
     }
 
-    private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega)
+    private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega,
+        RotateControlParams rotateParams)
     {
         var rotateActive = active && fleetMode == 2;
         if (!rotateActive)
@@ -274,15 +294,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         }
 
         _multiVehicleRotateModeActive = true;
-        if (Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega)
+        if (Math.Abs(requestedOmega) > rotateParams.ActiveOmega)
             _multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
     }
 
     private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
+        RotateControlParams rotateParams,
         float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
         bool rampStop = true)
     {
-        var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+        var hint = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
             ? Math.Sign(requestedOmega)
             : Math.Sign(_multiVehicleRotateDirectionHint);
         if (hint == 0) hint = 1;
@@ -291,13 +312,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         // Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero.
         if (rampStop)
             chassis.RampStop();
-        var alignOmega = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+        var alignOmega = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
             ? requestedOmega
             : 0.01f * hint;
         var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
             localCompensateX: localCompensateX, localCompensateY: localCompensateY,
             localCompensateTh: localCompensateTh);
-        var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+        var wheelAligned = TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
             out var alignDetail);
         var aligned = motionOk && wheelAligned;
         if (!motionOk)
@@ -315,13 +336,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             DLog.Log(
                 $"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " +
                 $"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
-                $"rampStop={rampStop} ok={motionOk} aligned={aligned} detail={alignDetail} " +
+                $"rampStop={rampStop} startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail} " +
                 $"chassisReason={chassis.LastMotionDecomposeFailureReason}",
                 "MultiVehicleRemoteDbg");
             FleetDiag(
                 $"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " +
                 $"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
-                $"ok={motionOk} aligned={aligned} detail={alignDetail}");
+                $"startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail}");
         }
 
         Hedingben.ToastText(
@@ -532,7 +553,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 $"car{CarNum} NOTIFY_APPLY seq={notification.Seq} mode={notification.Mode} " +
                 $"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
                 $"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
-                $"reason={notification.FleetStopReason}",
+                $"reason={notification.FleetStopReason} useDetourCorr={notification.UseDetourCorrection} " +
+                $"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
+                $"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
+                $"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
+                $"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
+                $"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})",
                 "MultiVehicleRemoteDbg");
         }
 
@@ -594,6 +620,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
         var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
         var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
+        VehicleSyncNotification activeNotification = null;
 
         if (isMaster)
             autoEnabled = MultiVehicleAutoEnabled;
@@ -607,11 +634,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                             Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
                 if (fresh && MultiVehicleNotification != null)
                 {
+                    activeNotification = MultiVehicleNotification;
                     autoEnabled = MultiVehicleNotification.AutoEnabled;
                     manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
                 }
             }
         }
+        var rotateParams = BuildRotateControlParams(isMaster ? null : activeNotification);
 
         // 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
         // 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
@@ -644,7 +673,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
             }
             _multiVehicleWasActive = false;
-            UpdateMultiVehicleRotateModeState(0, false, 0);
+            UpdateMultiVehicleRotateModeState(0, false, 0, rotateParams);
+            ResetMultiVehicleRotateCenterDrift();
             LogMultiVehicleRemoteDecision(
                 $"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
                 $"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
@@ -684,10 +714,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         //    false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
         //  - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
         //    SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
-        //    手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；确需手动 POS 纠偏时再开
+        //    手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；脚本自动原地旋转
-        //    MultiVehicleManualUseDetourCorrection。
+        //    (FleetRotateInPlace) 已经依赖 Detour 判停，因此显式打开 POS 纠偏以保持旋转中心。
         var autoMode = autoEnabled && !manualEnabled;
-        var manualDetourCorrection = manualEnabled && Conf.MultiVehicleManualUseDetourCorrection;
+        var scriptRotateDetourCorrection = isMaster && scriptOn && manualMode == 2 && Conf.FleetRotateUseDetourHeading;
+        var notificationDetourCorrection = !isMaster && activeNotification != null &&
+                                           activeNotification.UseDetourCorrection;
+        var manualDetourCorrection = manualEnabled &&
+                                     (Conf.MultiVehicleManualUseDetourCorrection ||
+                                      scriptRotateDetourCorrection ||
+                                      notificationDetourCorrection);
         var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
         var slamRead = useDetourCorrection || (isMaster && autoMode);
         float selfX = 0, selfY = 0, selfTh = 0;
@@ -825,7 +861,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 ? notificationRequestedFleetOmega
                 : fleetOmega)
             : fleetOmega;
-        UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega);
+        UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
 
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
 
@@ -1055,7 +1091,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 : syncDistance / 2f;
             chassis.ControlPointRadius = controlRadius;
             // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
-            chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;
+            chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;
 
             if (canMove && Conf.MultiVehicleUseDetect)
             {
@@ -1109,23 +1145,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
                 // 仅在"被指令旋转"时才纠偏：松开摇杆(fleetOmega≈0)时绝不再下发补偿，
                 // 否则积分残留会持续驱动车辆平移/旋转，表现为"松杆后轮子来回打、自转停不下来"。
-                var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
+                var rotating = Math.Abs(fleetOmega) > rotateParams.ActiveOmega;
                 if (canMove && rotating)
                 {
                     // #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
-                    var allowInteg = chassis.LastRotateAligned;
+                    var allowInteg = chassis.LastRotateAligned &&
+                                     MultiVehicleRotateWheelsReady &&
+                                     MultiVehicleRotateFleetReady &&
+                                     !rotateHoldForAlignment;
                     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,
+                        rotateParams.CompXyFac, rotateParams.CompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
+                        rotateParams.CompXyMax, dt, allowInteg);
                     rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
-                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
+                        rotateParams.CompXyFac, rotateParams.CompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
+                        rotateParams.CompXyMax, dt, allowInteg);
                     rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
-                        Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
+                        rotateParams.CompThFac, rotateParams.CompThIFac,
-                        Conf.MultiVehicleRotateCompThMax, dt, allowInteg);
+                        rotateParams.CompThMax, dt, allowInteg);
 
                     // #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
                     // 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
@@ -1140,6 +1179,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         rotCompVy *= compScale;
                         rotCompOmega *= compScale;
                     }
+                    LimitRotateCompensation(ref rotCompVx, ref rotCompVy, ref rotCompOmega,
+                        absOmega, controlRadius, rotateParams.CompTangentFrac);
                 }
                 else
                 {
@@ -1156,7 +1197,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 bool motionOk;
                 if (rotateHoldForAlignment)
                 {
-                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
                         rotCompVx, rotCompVy, rotCompOmega);
                 }
                 else if (rotating)
@@ -1164,7 +1205,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     // Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
                     if (!MultiVehicleRotateWheelsReady)
                     {
-                        motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                        motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
                             rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
                         rotateHoldForAlignment = true;
                         fleetOmega = 0;
@@ -1187,14 +1228,28 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         motionOk = chassis.SendRotateMotion(fleetOmega,
                             localCompensateX: rotCompVx, localCompensateY: rotCompVy,
                             localCompensateTh: rotCompOmega);
+                        var activeWheelAlignDeg = rotateParams.ActiveWheelAlignDeg;
                         var activeAligned = TryCheckRotateWheelAlignment(chassis,
-                            Conf.InPlaceRotateWheelAlignDeg, out _multiVehicleRotateAlignDetail);
+                            activeWheelAlignDeg, out _multiVehicleRotateAlignDetail);
                         if (!motionOk)
                             MultiVehicleRotateWheelsReady = false;
-                        if (!activeAligned)
+                        if (motionOk && !activeAligned)
+                        {
+                            MultiVehicleRotateWheelsReady = false;
+                            MultiVehicleRotateFleetReady = false;
+                            rotateHoldForAlignment = true;
+                            fleetOmega = 0;
+                            _rotIntegX = _rotIntegY = _rotIntegTh = 0;
+                            _rotPiLastTime = DateTime.MinValue;
+                            _rotOmegaPeak = 0;
+                            rotCompVx = rotCompVy = rotCompOmega = 0;
+                            _mvRotCompVx = _mvRotCompVy = _mvRotCompOmega = 0;
+                            motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams);
                             LogMultiVehicleRemoteDecision(
-                                $"ROTATE_ACTIVE_ALIGN_WAIT master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                                $"ROTATE_ACTIVE_REHOLD master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
-                                $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}");
+                                $"requestedOmega={requestedFleetOmega:0.000} activeTol={activeWheelAlignDeg:0.0} " +
+                                $"align={_multiVehicleRotateAlignDetail}", true);
+                        }
                     }
                 }
                 else
@@ -1203,7 +1258,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         localCompensateX: rotCompVx, localCompensateY: rotCompVy,
                         localCompensateTh: rotCompOmega);
                     MultiVehicleRotateWheelsReady = motionOk &&
-                                                    TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+                                                    TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
                                                         out _multiVehicleRotateAlignDetail);
                 }
                 SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
@@ -1216,11 +1271,22 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
                     fleetOmega);
+                if (fleetPosValid)
+                    LogMultiVehicleRotateCenterDrift(isMaster, manualEnabled, autoEnabled, fleetReady, canMove, rotating,
+                        CenterX, CenterY, CenterTh, requestedFleetOmega, fleetOmega,
+                        rotCompVx, rotCompVy, rotCompOmega);
+                else
+                    ResetMultiVehicleRotateCenterDrift();
                 LogMultiVehicleRemoteDecision(
                     $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
                     $"mode={fleetMode} omegaReq={requestedFleetOmega:0.000} omega={fleetOmega:0.000} " +
                     $"comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) align={_multiVehicleRotateAlignDetail} " +
+                    $"rotParam(xyP={rotateParams.CompXyFac:0.###} xyI={rotateParams.CompXyIFac:0.###} " +
+                    $"xyMax={rotateParams.CompXyMax:0.#} thP={rotateParams.CompThFac:0.###} " +
+                    $"thI={rotateParams.CompThIFac:0.###} thMax={rotateParams.CompThMax:0.#} " +
+                    $"frac={rotateParams.CompTangentFrac:0.###} activeOmega={rotateParams.ActiveOmega:0.###} " +
+                    $"startTol={rotateParams.StartWheelAlignDeg:0.#} activeTol={rotateParams.ActiveWheelAlignDeg:0.#}) " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
 
                 // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
@@ -1240,6 +1306,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _rotPiLastTime = DateTime.MinValue;
                 _rotPoseEpisode = false;
                 _rotPosePrevTime = DateTime.MinValue;
+                ResetMultiVehicleRotateCenterDrift();
                 // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
                 var motionOk = chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                     localCompensateX: xDetectCompensate + xPosCompensate,
@@ -1345,6 +1412,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         AutoEnabled = MultiVehicleAutoEnabled,
                         // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
                         ManualEnabled = manualEnabled,
+                        UseDetourCorrection = useDetourCorrection,
                         SyncTh = syncTh,
                         SyncDistance = syncDistance,
                         DeltaDetectCenter = deltaDetectCenter,
@@ -1357,13 +1425,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         IdealY = MultiVehicleAutoIdealY,
                         IdealTh = MultiVehicleAutoIdealTh
                     };
+                    FillRotateNotificationParams(notification, rotateParams);
                 }
 
                 LogMultiVehicleRemoteDecision(
                     $"NOTIFY_SEND seq={notification.Seq} mode={notification.Mode} " +
                     $"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
                     $"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
-                    $"reason={notification.FleetStopReason} fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum}");
+                    $"reason={notification.FleetStopReason} useDetourCorr={notification.UseDetourCorrection} " +
+                    $"fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum} " +
+                    $"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
+                    $"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
+                    $"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
+                    $"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
+                    $"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})");
 
                 foreach (var kv in notification.Fleet)
                 {
@@ -1467,12 +1542,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     {
         centerX = centerY = centerTh = 0;
         var carPos = DetourInterface.getCartLocation();
+        return TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
+            out centerX, out centerY, out centerTh);
+    }
+
+    public bool TryGetFleetCenterFromPose(float carX, float carY, float carTh,
+        out float centerX, out float centerY, out float centerTh)
+    {
+        centerX = centerY = centerTh = 0;
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
         var self = new VehicleSyncInfo
         {
-            X = (float)carPos.x,
+            X = carX,
-            Y = (float)carPos.y,
+            Y = carY,
-            Th = (float)carPos.th,
+            Th = carTh,
             LayoutX = layoutX,
             LayoutY = layoutY,
             LayoutTh = layoutTh
@@ -1663,6 +1746,149 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private static float ClampBias(float value, float threshold)
         => Math.Sign(value) * Math.Min(Math.Abs(value), threshold);
 
+    private RotateControlParams BuildRotateControlParams(VehicleSyncNotification notification)
+    {
+        var parameters = new RotateControlParams
+        {
+            ActiveOmega = Conf.MultiVehicleRotateActiveOmega,
+            CompXyFac = Conf.MultiVehicleRotateCompXyFac,
+            CompXyIFac = Conf.MultiVehicleRotateCompXyIFac,
+            CompXyMax = Conf.MultiVehicleRotateCompXyMax,
+            CompThFac = Conf.MultiVehicleRotateCompThFac,
+            CompThIFac = Conf.MultiVehicleRotateCompThIFac,
+            CompThMax = Conf.MultiVehicleRotateCompThMax,
+            CompTangentFrac = NormalizeRotateCompTangentFrac(Conf.MultiVehicleRotateCompTangentFrac),
+            StartWheelAlignDeg = Conf.InPlaceRotateWheelAlignDeg,
+            ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(Conf.InPlaceRotateActiveWheelAlignDeg,
+                Conf.InPlaceRotateWheelAlignDeg)
+        };
+
+        if (notification == null || !notification.RotateParamsValid)
+            return parameters;
+
+        parameters.ActiveOmega = notification.RotateActiveOmega;
+        parameters.CompXyFac = notification.RotateCompXyFac;
+        parameters.CompXyIFac = notification.RotateCompXyIFac;
+        parameters.CompXyMax = notification.RotateCompXyMax;
+        parameters.CompThFac = notification.RotateCompThFac;
+        parameters.CompThIFac = notification.RotateCompThIFac;
+        parameters.CompThMax = notification.RotateCompThMax;
+        parameters.CompTangentFrac = NormalizeRotateCompTangentFrac(notification.RotateCompTangentFrac);
+        parameters.StartWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateStartWheelAlignDeg,
+            Conf.InPlaceRotateWheelAlignDeg);
+        parameters.ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateActiveWheelAlignDeg,
+            parameters.StartWheelAlignDeg);
+        return parameters;
+    }
+
+    private static void FillRotateNotificationParams(VehicleSyncNotification notification, RotateControlParams parameters)
+    {
+        notification.RotateParamsValid = true;
+        notification.RotateActiveOmega = parameters.ActiveOmega;
+        notification.RotateCompXyFac = parameters.CompXyFac;
+        notification.RotateCompXyIFac = parameters.CompXyIFac;
+        notification.RotateCompXyMax = parameters.CompXyMax;
+        notification.RotateCompThFac = parameters.CompThFac;
+        notification.RotateCompThIFac = parameters.CompThIFac;
+        notification.RotateCompThMax = parameters.CompThMax;
+        notification.RotateCompTangentFrac = parameters.CompTangentFrac;
+        notification.RotateStartWheelAlignDeg = parameters.StartWheelAlignDeg;
+        notification.RotateActiveWheelAlignDeg = parameters.ActiveWheelAlignDeg;
+    }
+
+    private static float NormalizeRotateWheelAlignDeg(float value, float fallback)
+    {
+        if (float.IsNaN(value) || float.IsInfinity(value) || value <= 0)
+            return fallback;
+        return value;
+    }
+
+    private static float NormalizeRotateCompTangentFrac(float frac)
+    {
+        if (float.IsNaN(frac) || float.IsInfinity(frac) || frac < 0)
+            return DefaultRotateCompTangentFrac;
+        return frac;
+    }
+
+    private void LimitRotateCompensation(ref float compVx, ref float compVy, ref float compOmega,
+        float absOmega, float controlRadius, float tangentFrac)
+    {
+        var frac = NormalizeRotateCompTangentFrac(tangentFrac);
+        if (frac < 0 || absOmega <= 1e-6f) return;
+
+        var rawVx = compVx;
+        var rawVy = compVy;
+        var rawOmega = compOmega;
+        var tangentMmps = absOmega / 180f * (float)Math.PI * Math.Max(1f, Math.Abs(controlRadius));
+        var xyLimit = tangentMmps * frac;
+        var xyMag = (float)Math.Sqrt(compVx * compVx + compVy * compVy);
+        if (xyMag > xyLimit && xyMag > 1e-6f)
+        {
+            var scale = xyLimit / xyMag;
+            compVx *= scale;
+            compVy *= scale;
+        }
+
+        var omegaLimit = absOmega * frac;
+        compOmega = ClampBias(compOmega, omegaLimit);
+
+        var changed = Math.Abs(rawVx - compVx) > 1e-3f ||
+                      Math.Abs(rawVy - compVy) > 1e-3f ||
+                      Math.Abs(rawOmega - compOmega) > 1e-3f;
+        var now = DateTime.Now;
+        if (changed && (now - _mvRotateCompLimitLastLog).TotalMilliseconds >= 200)
+        {
+            _mvRotateCompLimitLastLog = now;
+            DLog.Log(
+                $"car{CarNum} ROTATE_COMP_LIMIT frac={frac:0.00} omega={absOmega:0.000} radius={controlRadius:0} " +
+                $"tan={tangentMmps:0.0} xyLimit={xyLimit:0.0} omLimit={omegaLimit:0.000} " +
+                $"raw=({rawVx:0.0},{rawVy:0.0},{rawOmega:0.000}) " +
+                $"limited=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
+                "MultiVehicleRemoteDbg");
+        }
+    }
+
+    private void ResetMultiVehicleRotateCenterDrift()
+    {
+        _mvRotateCenterDriftActive = false;
+        _mvRotateCenterMaxDrift = 0;
+        _mvRotateCenterLastLog = DateTime.MinValue;
+    }
+
+    private void LogMultiVehicleRotateCenterDrift(bool isMaster, bool manualEnabled, bool autoEnabled,
+        bool fleetReady, bool canMove, bool rotating, float centerX, float centerY, float centerTh,
+        float requestedOmega, float fleetOmega, float compVx, float compVy, float compOmega)
+    {
+        if (!_mvRotateCenterDriftActive)
+        {
+            _mvRotateCenterDriftActive = true;
+            _mvRotateCenterStartX = centerX;
+            _mvRotateCenterStartY = centerY;
+            _mvRotateCenterStartTh = centerTh;
+            _mvRotateCenterMaxDrift = 0;
+            _mvRotateCenterLastLog = DateTime.MinValue;
+        }
+
+        var dx = centerX - _mvRotateCenterStartX;
+        var dy = centerY - _mvRotateCenterStartY;
+        var drift = (float)Math.Sqrt(dx * dx + dy * dy);
+        _mvRotateCenterMaxDrift = Math.Max(_mvRotateCenterMaxDrift, drift);
+        var dth = (float)CommonMath.ThDiff(centerTh, _mvRotateCenterStartTh);
+        var now = DateTime.Now;
+        if ((now - _mvRotateCenterLastLog).TotalMilliseconds < 250)
+            return;
+
+        _mvRotateCenterLastLog = now;
+        DLog.Log(
+            $"car{CarNum} CTRL master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+            $"ready={fleetReady} canMove={canMove} rotating={rotating} " +
+            $"center=({centerX:0.0},{centerY:0.0},{centerTh:0.00}) " +
+            $"start=({_mvRotateCenterStartX:0.0},{_mvRotateCenterStartY:0.0},{_mvRotateCenterStartTh:0.00}) " +
+            $"drift=({dx:0.0},{dy:0.0}) dist={drift:0.0} max={_mvRotateCenterMaxDrift:0.0} dth={dth:0.00} " +
+            $"omegaReq={requestedOmega:0.000} omega={fleetOmega:0.000} comp=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
+            "FleetRotateCenterDbg");
+    }
+
     /// <summary>
     /// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
     /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。

MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs

@@ -55,6 +55,16 @@ internal static class VehicleSyncBinaryCodec
         writer.Write(notification.SyncTh);
         writer.Write(notification.SyncDistance);
         writer.Write(notification.DeltaDetectCenter);
+        writer.Write(notification.RotateActiveOmega);
+        writer.Write(notification.RotateCompXyFac);
+        writer.Write(notification.RotateCompXyIFac);
+        writer.Write(notification.RotateCompXyMax);
+        writer.Write(notification.RotateCompThFac);
+        writer.Write(notification.RotateCompThIFac);
+        writer.Write(notification.RotateCompThMax);
+        writer.Write(notification.RotateCompTangentFrac);
+        writer.Write(notification.RotateStartWheelAlignDeg);
+        writer.Write(notification.RotateActiveWheelAlignDeg);
         writer.Write(notification.IdealX);
         writer.Write(notification.IdealY);
         writer.Write(notification.IdealTh);
@@ -99,6 +109,16 @@ internal static class VehicleSyncBinaryCodec
         notification.SyncTh = reader.ReadSingle();
         notification.SyncDistance = reader.ReadSingle();
         notification.DeltaDetectCenter = reader.ReadSingle();
+        notification.RotateActiveOmega = reader.ReadSingle();
+        notification.RotateCompXyFac = reader.ReadSingle();
+        notification.RotateCompXyIFac = reader.ReadSingle();
+        notification.RotateCompXyMax = reader.ReadSingle();
+        notification.RotateCompThFac = reader.ReadSingle();
+        notification.RotateCompThIFac = reader.ReadSingle();
+        notification.RotateCompThMax = reader.ReadSingle();
+        notification.RotateCompTangentFrac = reader.ReadSingle();
+        notification.RotateStartWheelAlignDeg = reader.ReadSingle();
+        notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
         notification.IdealX = reader.ReadSingle();
         notification.IdealY = reader.ReadSingle();
         notification.IdealTh = reader.ReadSingle();
@@ -209,6 +229,8 @@ internal static class VehicleSyncBinaryCodec
         if (notification.AutoEnabled) flags |= 1 << 4;
         if (notification.ManualEnabled) flags |= 1 << 5;
         if (notification.HasIdeal) flags |= 1 << 6;
+        if (notification.RotateParamsValid) flags |= 1 << 7;
+        if (notification.UseDetourCorrection) flags |= 1 << 8;
         return flags;
     }
 
@@ -221,6 +243,8 @@ internal static class VehicleSyncBinaryCodec
         notification.AutoEnabled = (flags & (1 << 4)) != 0;
         notification.ManualEnabled = (flags & (1 << 5)) != 0;
         notification.HasIdeal = (flags & (1 << 6)) != 0;
+        notification.RotateParamsValid = (flags & (1 << 7)) != 0;
+        notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
     }
 
     private static void WriteString(BinaryWriter writer, string value)

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -47,9 +47,22 @@ public class VehicleSyncNotification
     [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
     [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
     [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
+    [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
     [JsonProperty("SyncTh")] public float SyncTh { get; set; }
     [JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
     [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
+    // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
+    [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
+    [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
+    [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
+    [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
+    [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
+    [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
+    [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
+    [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
+    [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
+    [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
+    [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
     // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
     [JsonProperty("Seq")] public long Seq { get; set; }
     // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。