Fix fleet rotate synchronization

MultiWheel/MultiWheelC/MovementTests.cs

@@ -212,7 +212,7 @@ public class FleetRotateInPlace : MovementDefinition
         if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
 
         DLog.Log(
-            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
+            $"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
             $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
             $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
             "FleetRotateDbg");
@@ -224,6 +224,35 @@ public class FleetRotateInPlace : MovementDefinition
         self.MultiVehicleScriptMode = 2;
         self.MultiVehicleScriptVth = 0;
         self.MultiVehicleScriptEnabled = true;
+        self.MultiVehicleRotateWheelsReady = false;
+        self.MultiVehicleRotateFleetReady = false;
+
+        DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
+        while (!self.MultiVehicleRotateFleetReady)
+        {
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptMode = 2;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleScriptEnabled = true;
+            Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
+            yield return true;
+        }
+
+        if (hasPos)
+        {
+            prevTh = (float)DetourInterface.getCartLocation().th;
+            startTh = prevTh;
+        }
+        accumulated = 0f;
+        start = DateTime.Now;
+        lastTime = start;
+        lastLog = 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");
 
         var stopReason = "stop()";
         while (true)

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -13,6 +13,7 @@ using ClumsyCore.Pilot;
 using ClumsyCore.Utilities;
 using CommonUsage;
 using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using FundamentalLib.Utilities;
 using MDCSToolBox.Clumsy.Pilot.MultiWheel;
@@ -99,6 +100,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private long _multiVehicleAppliedSeq = -1;
 
     [AsLowerIO(desc = "车号")] public int CarNum = 1;
+    [FieldMember(desc = "多车联动：原地旋转本车舵轮已对齐")]
+    public bool MultiVehicleRotateWheelsReady = true;
+    [FieldMember(desc = "多车联动：原地旋转整队舵轮已对齐")]
+    public bool MultiVehicleRotateFleetReady = true;
 
     #region 夹臂变量
     [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
@@ -138,6 +143,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private bool _multiVehicleMotionFeasible = true;
     private string _multiVehicleMotionInfeasibleReason = "";
     private DateTime _multiVehicleStopLastLog = DateTime.MinValue;
+    private bool _multiVehicleRotateModeActive;
+    private float _multiVehicleRotateDirectionHint = 1f;
+    private string _multiVehicleRotateAlignDetail = "";
+    private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;
 
     // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
     private DateTime _mvDbgLastLog = DateTime.MinValue;
@@ -167,6 +176,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
     private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
     private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
+    private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;
 
     private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
         int manualMode, float manualVx, float manualVy, float manualVth)
@@ -204,6 +214,118 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         _multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
     }
 
+    private static float ClampFloat(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega)
+    {
+        var rotateActive = active && fleetMode == 2;
+        if (!rotateActive)
+        {
+            _multiVehicleRotateModeActive = false;
+            MultiVehicleRotateWheelsReady = true;
+            MultiVehicleRotateFleetReady = true;
+            _multiVehicleRotateDirectionHint = 1f;
+            _multiVehicleRotateAlignDetail = "";
+            return;
+        }
+
+        if (!_multiVehicleRotateModeActive)
+        {
+            MultiVehicleRotateWheelsReady = false;
+            MultiVehicleRotateFleetReady = false;
+            _multiVehicleRotateAlignDetail = "rotate mode just entered";
+        }
+
+        _multiVehicleRotateModeActive = true;
+        if (Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega)
+            _multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
+    }
+
+    private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
+        float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
+        bool rampStop = true)
+    {
+        var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+            ? Math.Sign(requestedOmega)
+            : Math.Sign(_multiVehicleRotateDirectionHint);
+        if (hint == 0) hint = 1;
+        _multiVehicleRotateDirectionHint = hint;
+
+        // 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
+            ? requestedOmega
+            : 0.01f * hint;
+        var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
+            localCompensateX: localCompensateX, localCompensateY: localCompensateY,
+            localCompensateTh: localCompensateTh);
+        var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+            out var alignDetail);
+        var aligned = motionOk && wheelAligned;
+        if (!motionOk)
+            alignDetail = string.IsNullOrWhiteSpace(chassis.LastMotionDecomposeFailureReason)
+                ? alignDetail
+                : chassis.LastMotionDecomposeFailureReason;
+
+        MultiVehicleRotateWheelsReady = aligned;
+        _multiVehicleRotateAlignDetail = alignDetail;
+
+        var now = DateTime.Now;
+        if ((now - _multiVehicleRotateAlignLastLog).TotalMilliseconds >= 200)
+        {
+            _multiVehicleRotateAlignLastLog = now;
+            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} " +
+                $"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}");
+        }
+
+        Hedingben.ToastText(
+            aligned ? "车队原地旋转舵轮已对齐" : $"车队原地旋转舵轮预对齐中 {alignDetail}",
+            $"MultiVehicle{CarNum}-rotate-align");
+
+        return motionOk;
+    }
+
+    private static bool TryCheckRotateWheelAlignment(MultiWheelChassis chassis, float toleranceDeg, out string detail)
+    {
+        try
+        {
+#pragma warning disable CS0612, CS0618
+            var wheels = chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+            var maxDth = 0f;
+            var parts = new List<string>();
+            for (var i = 0; i < wheels.Count; i++)
+            {
+                var sw = wheels[i];
+                var dth = Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle()));
+                maxDth = Math.Max(maxDth, dth);
+                parts.Add($"w{i}:{dth:0.0}");
+            }
+
+            detail = $"maxDth={maxDth:0.0} tol={toleranceDeg:0.0} {string.Join(",", parts)}";
+            return maxDth <= toleranceDeg;
+        }
+        catch (Exception e)
+        {
+            detail = $"read alignment failed: {e.Message}";
+            return chassis.LastRotateAligned;
+        }
+    }
+
     private void LogMultiVehicleStop(string reason, bool force = false)
     {
         var now = DateTime.Now;
@@ -354,6 +476,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     MultiVehicleNotification = notification;
                     _multiVehicleLastNotifyTime = DateTime.Now;
                 }
+                var applyNow = DateTime.Now;
+                if ((applyNow - _mvNotifyApplyLastLog).TotalMilliseconds >= 200)
+                {
+                    _mvNotifyApplyLastLog = applyNow;
+                    DLog.Log(
+                        $"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}",
+                        "MultiVehicleRemoteDbg");
+                }
                 return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
             }
             catch (Exception e)
@@ -472,6 +605,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
             }
             _multiVehicleWasActive = false;
+            UpdateMultiVehicleRotateModeState(0, false, 0);
             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}");
@@ -535,6 +669,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         var notificationStopActive = false;
         var notificationStopReason = "";
         var notificationStopSourceCar = 0;
+        var notificationRequestedFleetOmega = 0f;
+        var notificationMotionReleased = true;
         float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
         float crabSteerLimit = 0;
         var crabReverseEquivalent = false;
@@ -548,7 +684,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 if (fleetMode == 2)
                 {
                     // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
-                    fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
+                    var rotateInput = scriptOn ? manualVth : ClampFloat(manualVth, -1f, 1f);
+                    fleetOmega = scriptOn ? rotateInput : rotateInput * Conf.ManualCarSyncVthFac;
                     fleetVx = 0;
                     fleetFrontTh = 0;
                     fleetRearTh = 0;
@@ -631,6 +768,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             fleetRearTh = notification.FleetRearTh;
             fleetMode = notification.Mode;
             fleetOmega = notification.FleetOmega;
+            notificationRequestedFleetOmega = notification.RequestedFleetOmega;
+            notificationMotionReleased = notification.FleetMotionReleased;
             notificationStopActive = notification.FleetStopActive;
             notificationStopReason = notification.FleetStopReason ?? "";
             notificationStopSourceCar = notification.FleetStopSourceCar;
@@ -641,6 +780,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             MultiVehicleAutoIdealTh = notification.IdealTh;
         }
 
+        var requestedFleetOmega = !isMaster && fleetMode == 2
+            ? ((Math.Abs(notificationRequestedFleetOmega) > 1e-6f || !notificationMotionReleased)
+                ? notificationRequestedFleetOmega
+                : fleetOmega)
+            : fleetOmega;
+        UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega);
+
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
 
         if (isMaster)
@@ -716,6 +862,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         bool othersDetectOk;
         string otherDetectLostCars;
         List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
+        List<KeyValuePair<int, VehicleSyncInfo>> rotateUnalignedMembers;
         lock (FleetLock)
         {
             othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
@@ -725,7 +872,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             motionInfeasibleMembers = MultiVehicleFleet
                 .Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
                 .ToList();
+            rotateUnalignedMembers = MultiVehicleFleet
+                .Where(kv => !kv.Value.RotateWheelsAligned)
+                .ToList();
         }
+        if (!MultiVehicleRotateWheelsReady && rotateUnalignedMembers.All(kv => kv.Key != CarNum))
+        {
+            rotateUnalignedMembers.Add(new KeyValuePair<int, VehicleSyncInfo>(CarNum, new VehicleSyncInfo
+            {
+                RotateWheelsAligned = false,
+                RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            }));
+        }
+        var rotateUnalignedCars = string.Join(",", rotateUnalignedMembers.Select(kv => kv.Key.ToString()));
+        var rotateFleetWheelsAligned = fleetMode != 2 || (fleetReady && rotateUnalignedMembers.Count == 0);
+        MultiVehicleRotateFleetReady = rotateFleetWheelsAligned;
 
         var fleetStopActive = false;
         var fleetStopReason = "";
@@ -794,6 +955,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
         }
 
+        var rotateHoldForAlignment = false;
+        if (fleetMode == 2 && fleetReady && !fleetStopActive && !notificationMotionReleased)
+        {
+            rotateHoldForAlignment = true;
+            fleetOmega = 0;
+            Hedingben.ToastText("车队原地旋转等待主车释放速度", $"MultiVehicle{CarNum}-rotate-align");
+            LogMultiVehicleRemoteDecision(
+                $"ROTATE_HOLD_RELEASE master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                $"requestedOmega={requestedFleetOmega:0.000}", true);
+        }
+        else if (fleetMode == 2 && fleetReady && !fleetStopActive && !rotateFleetWheelsAligned)
+        {
+            rotateHoldForAlignment = true;
+            fleetOmega = 0;
+            Hedingben.ToastText($"车队原地旋转舵轮预对齐中 car=[{rotateUnalignedCars}]", $"MultiVehicle{CarNum}-rotate-align");
+            LogMultiVehicleRemoteDecision(
+                $"ROTATE_HOLD_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                $"requestedOmega={requestedFleetOmega:0.000} unalignedCars=[{rotateUnalignedCars}]", true);
+        }
+
         if (fleetStopActive)
             LogMultiVehicleStop(fleetStopReason);
 
@@ -814,6 +995,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         {
             chassis.RampStop();
             SetMultiVehicleMotionFeasible(true);
+            if (fleetMode == 2)
+            {
+                MultiVehicleRotateWheelsReady = false;
+                MultiVehicleRotateFleetReady = false;
+                _multiVehicleRotateAlignDetail = $"fleet stop: {fleetStopReason}";
+            }
             LogMultiVehicleRemoteDecision(
                 $"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
                 $"reason={fleetStopReason}", true);
@@ -926,8 +1113,45 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _mvRotCompVy = rotCompVy;
                 _mvRotCompOmega = rotCompOmega;
 
-                var motionOk = chassis.SendRotateMotion(fleetOmega,
+                bool motionOk;
-                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+                if (rotateHoldForAlignment)
+                {
+                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                        rotCompVx, rotCompVy, rotCompOmega);
+                }
+                else if (rotating)
+                {
+                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                        rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
+                    if (motionOk && MultiVehicleRotateWheelsReady)
+                    {
+                        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);
+                    }
+                }
+                else
+                {
+                    motionOk = chassis.SendRotateMotion(fleetOmega,
+                        localCompensateX: rotCompVx, localCompensateY: rotCompVy,
+                        localCompensateTh: rotCompOmega);
+                    MultiVehicleRotateWheelsReady = motionOk &&
+                                                    TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+                                                        out _multiVehicleRotateAlignDetail);
+                }
                 SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
                 if (!motionOk)
                 {
@@ -938,7 +1162,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 LogMultiVehicleRemoteDecision(
                     $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
-                    $"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
+                    $"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} " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
 
                 // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
@@ -950,6 +1176,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _mvRotCompVx = 0;
                 _mvRotCompVy = 0;
                 _mvRotCompOmega = 0;
+                MultiVehicleRotateWheelsReady = true;
+                MultiVehicleRotateFleetReady = true;
+                _multiVehicleRotateAlignDetail = "";
                 // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
                 _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                 _rotPiLastTime = DateTime.MinValue;
@@ -1003,7 +1232,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
                 $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
                 $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
-                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2})";
+                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +
+                $"| ROT_ALIGN hold:{rotateHoldForAlignment} wheelReady:{MultiVehicleRotateWheelsReady} fleetReady:{MultiVehicleRotateFleetReady} " +
+                $"unaligned:[{rotateUnalignedCars}] reqOmega:{requestedFleetOmega:F1} detail:{_multiVehicleRotateAlignDetail}";
 
             DLog.Log(dbg, "MultiVehicleDbg");
             FleetDiag(dbg);
@@ -1046,6 +1277,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
                         Mode = fleetMode,
                         FleetOmega = fleetStopActive ? 0 : fleetOmega,
+                        RequestedFleetOmega = fleetMode == 2 && !fleetStopActive ? requestedFleetOmega : 0,
+                        FleetMotionReleased = !fleetStopActive && !(fleetMode == 2 && rotateHoldForAlignment),
                         FleetStopActive = fleetStopActive,
                         FleetStopReason = fleetStopReason,
                         FleetStopSourceCar = fleetStopSourceCar,
@@ -1066,6 +1299,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     };
                 }
 
+                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}");
+
                 foreach (var kv in notification.Fleet)
                 {
                     var ip = kv.Value.Ip;
@@ -1240,7 +1479,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             Aligned = aligned,
             DetectOk = detectOk,
             MotionFeasible = _multiVehicleMotionFeasible,
-            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason
+            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
+            RotateWheelsAligned = MultiVehicleRotateWheelsReady,
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
         };
     }
 

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -21,6 +21,8 @@ public class VehicleSyncInfo
     [JsonProperty("DetectOk")] public bool DetectOk { get; set; }
     [JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
     [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
+    [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
+    [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
 }
 
 public class VehicleSyncNotification
@@ -38,6 +40,8 @@ public class VehicleSyncNotification
     [JsonProperty("Mode")] public int Mode { get; set; }
     // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
     [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
+    [JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
+    [JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
     [JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
     [JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
     [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }