新增1.0和2.0两种协议车型车型

2026-07-17 13:36:01 +08:00
parent a0dc1e6cd0
commit a28bc68e23
62 changed files with 12638 additions and 203 deletions
@@ -0,0 +1,131 @@
+using System;
+
+namespace StandardScene.Fass2Simulator
+{
+    public sealed class Fass2SimCommandHandler
+    {
+        private readonly Fass2SimVehicle _vehicle;
+        private readonly Fass2SimMotionEngine _motion;
+        private readonly Fass2SimActionEngine _actions;
+        private readonly bool _logRawFrames;
+        private long _stateResponseCount;
+
+        public Fass2SimCommandHandler(
+            Fass2SimVehicle vehicle,
+            Fass2SimMotionEngine motion,
+            Fass2SimActionEngine actions,
+            bool logRawFrames)
+        {
+            _vehicle = vehicle;
+            _motion = motion;
+            _actions = actions;
+            _logRawFrames = logRawFrames;
+        }
+
+        public void Handle(byte[] packet, string remote)
+        {
+            if (!Fass2SimProtocol.TryGetCommand(packet, out var command, out var expectedLength))
+            {
+                Fass2SimLog.WriteLine($"[{Now()}] RX 非法报文 from {remote}, len={packet?.Length ?? 0}");
+                return;
+            }
+
+            if (packet.Length != expectedLength)
+            {
+                Fass2SimLog.WriteLine(
+                    $"[{Now()}] RX 长度不匹配 from {remote}: cmd={Fass2SimProtocol.CommandName(command)} actual={packet.Length} expected={expectedLength}");
+                return;
+            }
+
+            if (packet[expectedLength - 1] != Fass2SimProtocol.End)
+            {
+                Fass2SimLog.WriteLine($"[{Now()}] RX 帧尾错误 from {remote}: cmd={Fass2SimProtocol.CommandName(command)}");
+                return;
+            }
+
+            _vehicle.RecordCommandReceived();
+            _vehicle.LastCommand = command;
+
+            if (command == Fass2SimProtocol.CmdStateResponse)
+            {
+                _vehicle.RecordAckReceived();
+                LogStateResponseAck(remote);
+                return;
+            }
+
+            if (_logRawFrames)
+            {
+                Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}: {Fass2SimProtocol.ToHex(packet)}");
+            }
+            else
+            {
+                Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}");
+            }
+
+            switch (command)
+            {
+                case Fass2SimProtocol.CmdStart:
+                    _vehicle.State = 1;
+                    _motion.OnStartCommand();
+                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
+                    break;
+                case Fass2SimProtocol.CmdStop:
+                    _vehicle.State = 2;
+                    _motion.OnStopCommand();
+                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
+                    break;
+                case Fass2SimProtocol.CmdEmergencyStop:
+                    _vehicle.State = 3;
+                    _motion.OnStopCommand();
+                    Fass2SimLog.WriteLine($"[{Now()}]    -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
+                    break;
+                case Fass2SimProtocol.CmdQuery:
+                    Fass2SimLog.WriteLine($"[{Now()}]    -> TCP 查询（UDP 模式通常不发 0x00）");
+                    break;
+                case Fass2SimProtocol.CmdNodes:
+                    if (Fass2SimProtocol.TryParseNodes(packet, out var taskId, out var nodes))
+                    {
+                        _motion.OnNodesCommand(taskId, nodes);
+                    }
+                    else
+                    {
+                        Fass2SimLog.WriteLine($"[{Now()}]    -> 0xB1 解析失败");
+                    }
+                    break;
+                case Fass2SimProtocol.CmdAction:
+                    if (Fass2SimProtocol.TryParseAction(packet, out var actionId, out var patch))
+                    {
+                        _actions.OnActionCommand(actionId, patch);
+                    }
+                    else
+                    {
+                        Fass2SimLog.WriteLine($"[{Now()}]    -> 0xA1 解析失败");
+                    }
+                    break;
+                default:
+                    Fass2SimLog.WriteLine($"[{Now()}]    -> 暂未处理");
+                    break;
+            }
+        }
+
+        private void LogStateResponseAck(string remote)
+        {
+            _stateResponseCount++;
+            if (_stateResponseCount == 1)
+            {
+                Fass2SimLog.WriteLine($"[{Now()}] RX StateResponse(0x10) from {remote}，调度在线（后续 0x10 仅写文件日志）");
+                return;
+            }
+
+            if (_stateResponseCount % 200 == 0)
+            {
+                Fass2SimLog.WriteFileOnly($"[{Now()}] 0x10 累计应答 {_stateResponseCount} 次");
+            }
+        }
+
+        private static string Now()
+        {
+            return DateTime.Now.ToString("HH:mm:ss.fff");
+        }
+    }
+}