Fix FleetCrabWalk startup synchronization

MultiWheel/MultiWheelC/AGV.cs

@@ -2,6 +2,7 @@ using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Sensors;
 using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
 using MDCSToolBox.Clumsy.MotionControllers;
@@ -292,18 +293,13 @@ namespace MultiWheelC
                 RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
             }.Get()).Wait();
         }
-        // 车队联动-自动蟹行：供调度按绝对起终点触发。
-        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
         public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
-            float speed, float CarDirectionBias)
+            float speed)
         {
             var dx = dstX - srcX;
             var dy = dstY - srcY;
             var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
-            DLog.Log(
-                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
-                $"len={pathLength:0.0}, speed={speed:0.000}, CarDirectionBias={CarDirectionBias:0.0})",
-                "FleetCrabDbg");
 
             if (pathLength <= 1f)
             {
@@ -311,6 +307,23 @@ namespace MultiWheelC
                 return;
             }
 
+            var self = PilotDefinition.Self;
+            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
+                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+            {
+                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
+                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
+                return;
+            }
+
+            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
+            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0})",
+                "FleetCrabDbg");
+
             if (dstId != -1)
             {
                 while (!TryLock(dstId))
@@ -322,14 +335,10 @@ namespace MultiWheelC
 
             var action = new MultiWheelC.FleetCrabWalk
             {
-                UseAbsolutePath = true,
-                PathStartX = srcX,
-                PathStartY = srcY,
-                PathEndX = dstX,
-                PathEndY = dstY,
+                CrabAngleDeg = crabAngle,
+                BodyToPathAngleDeg = crabAngle,
                 CrabLengthMm = pathLength,
                 CrabSpeed = speed,
-                BodyToPathAngleDeg = -CarDirectionBias,
                 FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
                 FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
                 FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
@@ -352,7 +361,6 @@ namespace MultiWheelC
             }
         }
         
-
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
         {
             DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");

MultiWheel/MultiWheelC/MovementTests.cs

@@ -447,14 +447,6 @@ public class FleetCrabWalk : MovementDefinition
     /// <summary>路径长度(mm)。</summary>
     public float CrabLengthMm = 2000f;
 
-    /// <summary>使用调度给定的绝对起终点路径，而不是用当前车队中心和 CrabAngleDeg 生成路径。</summary>
-    public bool UseAbsolutePath = false;
-
-    public float PathStartX;
-    public float PathStartY;
-    public float PathEndX;
-    public float PathEndY;
-
     /// <summary>行驶速度(m/s)。</summary>
     public float CrabSpeed = 0.2f;
 
@@ -663,7 +655,7 @@ public class FleetCrabWalk : MovementDefinition
             $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
             $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
             $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
-            $"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
             $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
             $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
             "FleetCrabDbg");
@@ -687,40 +679,17 @@ public class FleetCrabWalk : MovementDefinition
 
         DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
 
-        Vector2 pathStart;
-        Vector2 dst;
-        float pathLengthMm;
-        float phi;
-        if (UseAbsolutePath)
-        {
-            pathStart = new Vector2(PathStartX, PathStartY);
-            dst = new Vector2(PathEndX, PathEndY);
-            var pathVec = dst - pathStart;
-            pathLengthMm = pathVec.Length();
-            if (pathLengthMm <= 1f)
-            {
-                DLog.Log(
-                    $"ABORT: absolute path is too short src=({PathStartX:0},{PathStartY:0}) dst=({PathEndX:0},{PathEndY:0}) len={pathLengthMm:0.0}",
-                    "FleetCrabDbg");
-                Hedingben.ToastText("车队蟹行路径长度过短", "FleetCrab");
-                yield break;
-            }
-            phi = CommonMath.RoundTh((float)(Math.Atan2(pathVec.Y, pathVec.X) / Math.PI * 180.0));
-        }
-        else
-        {
-            pathStart = new Vector2(x0, y0);
-            pathLengthMm = CrabLengthMm;
-            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
-            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
-        }
+        var pathStart = new Vector2(x0, y0);
+        var pathLengthMm = CrabLengthMm;
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
         var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
         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}) pathMode={(UseAbsolutePath ? "absolute" : "relative")} " +
+            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
             $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
             $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
             $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
@@ -728,6 +697,16 @@ public class FleetCrabWalk : MovementDefinition
             $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
             "FleetCrabDbg");
 
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+            ? conf.MultiVehicleControlRadius
+            : conf.TestCarSyncDistance / 2f));
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
+        var warmStart = DateTime.Now;
+        var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
         self.MultiVehicleScriptEnabled = false;
         self.MultiVehicleScriptMode = 0;
         self.MultiVehicleScriptVx = 0;
@@ -735,19 +714,23 @@ public class FleetCrabWalk : MovementDefinition
         self.MultiVehicleScriptVth = 0;
         self.MultiVehicleAutoEnabled = true;
         self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
         self.MultiVehicleAutoIdealX = pathStart.X;
         self.MultiVehicleAutoIdealY = pathStart.Y;
         self.MultiVehicleAutoIdealTh = targetBodyTh;
         self.MultiVehicleAutoHasIdeal = true;
         self.MultiVehicleAutoCmdTime = DateTime.Now;
         self.PrimeMasterAutoFromSlam();
-        DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
+        DLog.Log(
+            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");
 
-        var warmEnd = DateTime.Now.AddSeconds(2.0);
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
         var warmIter = 0;
         var warmReady = false;
+        var warmDetail = "";
         while (!_stopping && DateTime.Now < warmEnd)
         {
             warmIter++;
@@ -755,8 +738,8 @@ public class FleetCrabWalk : MovementDefinition
             self.MultiVehicleScriptMode = 0;
             self.MultiVehicleAutoEnabled = true;
             self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = 0;
-            self.MultiVehicleAutoRearTh = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
             self.MultiVehicleAutoIdealX = pathStart.X;
             self.MultiVehicleAutoIdealY = pathStart.Y;
             self.MultiVehicleAutoIdealTh = targetBodyTh;
@@ -769,26 +752,32 @@ 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} " +
-                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
+                    $"detail={warmDetail}",
                     "FleetCrabDbg");
-            if (cnt >= conf.MultiVehicleFleetNum)
+            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
             {
                 warmReady = true;
                 DLog.Log(
-                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
                     "FleetCrabDbg");
                 break;
             }
             yield return true;
         }
         if (!warmReady)
-            DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
+        {
+            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
                 "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+            Cleanup();
+            yield break;
+        }
 
         Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
 
-        if (warmReady && !UseAbsolutePath &&
-            self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
         {
             x0 = warmX;
             y0 = warmY;
@@ -819,13 +808,6 @@ public class FleetCrabWalk : MovementDefinition
         var accel = Math.Abs(FleetCrabAccel);
         var cmdSpeed = 0f;
         var lastTick = DateTime.Now;
-        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
-            ? conf.MultiVehicleControlRadius
-            : conf.TestCarSyncDistance / 2f));
-        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
-            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
-            out _, out _);
         var stopReason = "done";
 
         while (!_stopping)

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -185,6 +185,12 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
     public float FleetCrabGcpThetaThreshold = 95f;
 
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
     // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -280,10 +280,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         if (!rotateActive)
         {
             _multiVehicleRotateModeActive = false;
-            MultiVehicleRotateWheelsReady = true;
+            if (!active)
+            {
+                MultiVehicleRotateWheelsReady = true;
+                _multiVehicleRotateAlignDetail = "";
+            }
             MultiVehicleRotateFleetReady = true;
             _multiVehicleRotateDirectionHint = 1f;
-            _multiVehicleRotateAlignDetail = "";
             return;
         }
 
@@ -1309,10 +1312,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _mvRotCompVx = 0;
                 _mvRotCompVy = 0;
                 _mvRotCompOmega = 0;
-                MultiVehicleRotateWheelsReady = true;
                 MultiVehicleRotateFleetReady = true;
-                _multiVehicleRotateAlignDetail = "";
-                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
+                // Reset rotate-only PI state outside rotate mode.
                 _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                 _rotPiLastTime = DateTime.MinValue;
                 _rotPoseEpisode = false;
@@ -1323,6 +1324,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     localCompensateX: xDetectCompensate + xPosCompensate,
                     localCompensateY: yDetectCompensate + yPosCompensate,
                     localCompensateTh: thDetectCompensate + thPosCompensate);
+                if (motionOk)
+                    MultiVehicleRotateWheelsReady = TryCheckRotateWheelAlignment(chassis,
+                        Math.Max(0.1f, Conf.FleetCrabStartWheelAlignDeg), out _multiVehicleRotateAlignDetail);
+                else
+                {
+                    MultiVehicleRotateWheelsReady = false;
+                    _multiVehicleRotateAlignDetail = chassis.LastMotionDecomposeFailureReason;
+                }
+                MultiVehicleRotateFleetReady = true;
                 SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
                 if (!motionOk)
                 {
@@ -1335,6 +1345,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
                     $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
+                    $"wheelReady={MultiVehicleRotateWheelsReady} align={_multiVehicleRotateAlignDetail} " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
             }
         }
@@ -1631,15 +1642,64 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return true;
     }
 
-    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
+    public long BeginFleetMotionWarmup()
     {
-        // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
-        // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
+        MultiVehicleRotateWheelsReady = false;
+        MultiVehicleRotateFleetReady = false;
+        _multiVehicleRotateAlignDetail = "fleet motion warmup pending";
+        return Interlocked.Read(ref _multiVehicleNotifySeq);
+    }
+
+    public bool IsFleetMotionWarmupReady(DateTime warmStartTime, long notificationSeqBaseline,
+        float syncTh, float syncDistance, out string detail)
+    {
+        var pending = new List<string>();
+        var requiredCount = Math.Max(1, Conf.MultiVehicleFleetNum);
+        lock (FleetLock)
+        {
+            if (MultiVehicleFleet.Count != requiredCount)
+                pending.Add($"fleetCnt={MultiVehicleFleet.Count}/{requiredCount}");
+
+            foreach (var kv in MultiVehicleFleet.OrderBy(k => k.Key))
+            {
+                var carNum = kv.Key;
+                var info = kv.Value;
+                if (!_multiVehicleFleetSeen.TryGetValue(carNum, out var seen) || seen < warmStartTime)
+                    pending.Add($"car{carNum}:notFresh");
+
+                var (layoutX, layoutY, layoutTh) = GetLayoutPoseForCar(carNum, syncTh, syncDistance);
+                if (Math.Abs(info.LayoutX - layoutX) > 1f ||
+                    Math.Abs(info.LayoutY - layoutY) > 1f ||
+                    Math.Abs(CommonMath.ThDiff(info.LayoutTh, layoutTh)) > 1f)
+                    pending.Add(
+                        $"car{carNum}:layout=({info.LayoutX:0},{info.LayoutY:0},{info.LayoutTh:0.0})");
+
+                if (!info.MotionFeasible)
+                    pending.Add($"car{carNum}:motion={info.MotionInfeasibleReason}");
+                if (!info.RotateWheelsAligned)
+                    pending.Add($"car{carNum}:wheel={info.RotateWheelAlignDetail}");
+                if (carNum != CarNum && info.AppliedNotificationSeq <= notificationSeqBaseline)
+                    pending.Add($"car{carNum}:seq={info.AppliedNotificationSeq}<={notificationSeqBaseline}");
+            }
+        }
+
+        detail = pending.Count == 0
+            ? $"ready seqBase={notificationSeqBaseline}"
+            : string.Join("; ", pending);
+        return pending.Count == 0;
+    }
+
+    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
+        => GetLayoutPoseForCar(CarNum, syncTh, syncDistance);
+
+    private static (float, float, float) GetLayoutPoseForCar(int carNum, float syncTh, float syncDistance)
+    {
+        // Two-car layout: members are mirrored around the fleet center; car2 faces 180 deg away.
         var rad = syncTh / 180f * Math.PI;
-        var sign = CarNum == 1 ? 1f : -1f;
+        var sign = carNum == 1 ? 1f : -1f;
         var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign);
         var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign);
-        return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180));
+        return (xx, yy, syncTh + (carNum == 1 ? 0 : 180));
     }
 
     private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
@@ -1664,7 +1724,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             MotionFeasible = _multiVehicleMotionFeasible,
             MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
             RotateWheelsAligned = MultiVehicleRotateWheelsReady,
-            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail,
+            AppliedNotificationSeq = _multiVehicleAppliedSeq
         };
     }
 

MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs

@@ -7,7 +7,7 @@ namespace MultiWheelC;
 
 internal static class VehicleSyncBinaryCodec
 {
-    private const byte Version = 1;
+    private const byte Version = 2;
     private const byte RegisterType = 1;
     private const byte NotificationType = 2;
     private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
@@ -27,9 +27,9 @@ internal static class VehicleSyncBinaryCodec
     {
         using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
         using var reader = new BinaryReader(stream, Encoding.UTF8);
-        ReadHeader(reader, RegisterType);
+        var version = ReadHeader(reader, RegisterType);
         var carNum = reader.ReadInt32();
-        var info = ReadInfo(reader);
+        var info = ReadInfo(reader, version);
         EnsureFullyRead(stream);
         return (carNum, info);
     }
@@ -88,7 +88,7 @@ internal static class VehicleSyncBinaryCodec
     {
         using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
         using var reader = new BinaryReader(stream, Encoding.UTF8);
-        ReadHeader(reader, NotificationType);
+        var version = ReadHeader(reader, NotificationType);
 
         var notification = new VehicleSyncNotification
         {
@@ -129,7 +129,7 @@ internal static class VehicleSyncBinaryCodec
         for (var i = 0; i < fleetCount; ++i)
         {
             var carNum = reader.ReadInt32();
-            notification.Fleet[carNum] = ReadInfo(reader);
+            notification.Fleet[carNum] = ReadInfo(reader, version);
         }
 
         EnsureFullyRead(stream);
@@ -144,7 +144,7 @@ internal static class VehicleSyncBinaryCodec
         writer.Write((ushort)0);
     }
 
-    private static void ReadHeader(BinaryReader reader, byte expectedType)
+    private static byte ReadHeader(BinaryReader reader, byte expectedType)
     {
         for (var i = 0; i < Magic.Length; ++i)
         {
@@ -153,7 +153,7 @@ internal static class VehicleSyncBinaryCodec
         }
 
         var version = reader.ReadByte();
-        if (version != Version)
+        if (version < 1 || version > Version)
             throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
 
         var type = reader.ReadByte();
@@ -163,6 +163,8 @@ internal static class VehicleSyncBinaryCodec
         var reserved = reader.ReadUInt16();
         if (reserved != 0)
             throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
+
+        return version;
     }
 
     private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
@@ -178,9 +180,10 @@ internal static class VehicleSyncBinaryCodec
         writer.Write(info.LayoutTh);
         WriteString(writer, info.MotionInfeasibleReason);
         WriteString(writer, info.RotateWheelAlignDetail);
+        writer.Write(info.AppliedNotificationSeq);
     }
 
-    private static VehicleSyncInfo ReadInfo(BinaryReader reader)
+    private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
     {
         var info = new VehicleSyncInfo();
         ApplyInfoFlags(info, reader.ReadUInt16());
@@ -194,6 +197,7 @@ internal static class VehicleSyncBinaryCodec
         info.LayoutTh = reader.ReadSingle();
         info.MotionInfeasibleReason = ReadString(reader);
         info.RotateWheelAlignDetail = ReadString(reader);
+        info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
         return info;
     }
 

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -23,6 +23,7 @@ public class VehicleSyncInfo
     [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
     [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
     [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
+    [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
 }
 
 public class VehicleSyncNotification