2.0协议完善加简单交管配置

StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs

@@ -41,6 +41,12 @@ namespace StandardScene.Fass2Simulator
                 _windowNodes = nodes;
                 _vehicle.Task = taskId;
 
+                if (taskChanged)
+                {
+                    _actions.ResetForNewTask();
+                    ResetSegment();
+                }
+
                 ApplyCurrentNodeFields(nodes);
                 LogNodes(nodes);
 
@@ -63,10 +69,35 @@ namespace StandardScene.Fass2Simulator
                     }
                 }
 
+                if (!ContainsNode(nodes, _vehicle.Node))
+                {
+                    Fass2SimLog.WriteLine(
+                        $"[{Now()}]    -> 0xB1 窗口不含当前 node={_vehicle.Node}，忽略启动路段（避免回退到窗口首站）");
+                    return;
+                }
+
                 TryStartNextSegment();
             }
         }
 
+        private static bool ContainsNode(Fass2SimNodeMessage[] nodes, ushort node)
+        {
+            if (nodes == null)
+            {
+                return false;
+            }
+
+            for (var i = 0; i < nodes.Length; i++)
+            {
+                if (nodes[i].Node == node)
+                {
+                    return true;
+                }
+            }
+
+            return false;
+        }
+
         public void OnStartCommand()
         {
             lock (_syncRoot)
@@ -147,7 +178,7 @@ namespace StandardScene.Fass2Simulator
             }
 
             _segmentTarget = target;
-            _segmentLength = ResolveSegmentLength(target);
+            _segmentLength = ResolveSegmentLength(_vehicle.Node, target);
             _segmentProgress = 0;
             _segmentActive = true;
             _vehicle.State = 1;
@@ -184,7 +215,7 @@ namespace StandardScene.Fass2Simulator
             }
 
             var expected = _actions.ExpectedStation;
-            if (expected == null || expected.StartStop != Fass2SimActionResolver.StartStopPass)
+            if (expected == null || !Fass2SimActionResolver.AllowsAutoContinue(expected.StartStop))
             {
                 return;
             }
@@ -218,15 +249,24 @@ namespace StandardScene.Fass2Simulator
                 return null;
             }
 
+            // 必须先在窗口内定位当前站，再取其后一站。
+            // 旧逻辑在窗口不含当前站时会落到 nodes[0]，把更早的站当成下一目标 → 车体 Node 瞬间回退/瞬移。
+            var foundCurrent = false;
             var startIndex = 0;
             for (var i = 0; i < nodes.Length; i++)
             {
                 if (nodes[i].Node == currentNode)
                 {
+                    foundCurrent = true;
                     startIndex = i + 1;
                 }
             }
 
+            if (!foundCurrent)
+            {
+                return null;
+            }
+
             for (var i = startIndex; i < nodes.Length; i++)
             {
                 var node = nodes[i];
@@ -248,12 +288,21 @@ namespace StandardScene.Fass2Simulator
                 if (node.Node == _vehicle.Node && node.StartStop > 0)
                 {
                     _vehicle.StartStop = node.StartStop;
+                    _actions.MergeExpected(node);
                 }
             }
         }
 
-        private double ResolveSegmentLength(Fass2SimNodeMessage target)
+        private double ResolveSegmentLength(ushort fromNode, Fass2SimNodeMessage target)
         {
+            foreach (var node in _windowNodes)
+            {
+                if (node.Node == fromNode && node.Distance > 0)
+                {
+                    return node.Distance;
+                }
+            }
+
             if (target.Distance > 0)
             {
                 return target.Distance;
@@ -272,12 +321,12 @@ namespace StandardScene.Fass2Simulator
         {
             if (target.Speed > 0)
             {
-                return target.Speed;
+                return Fass2SimProtocol.ProtocolSpeedToMmPerSec(target.Speed);
             }
 
             if (_vehicle.Speed > 0)
             {
-                return _vehicle.Speed;
+                return Fass2SimProtocol.ProtocolSpeedToMmPerSec(_vehicle.Speed);
             }
 
             return _config.DefaultSpeed;