新增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,235 @@
+using SimpleCore.Library;
+using System;
+using System.Collections.Generic;
+using System.Net;
+using System.Net.Sockets;
+using System.Threading;
+
+namespace StandardScene.Magnetic.Protocol
+{
+    /// <summary>
+    /// FASS 2.0 PCB/UDP 共享监听枢纽。
+    /// AGV 主动上报 100B 状态 → 解析并分发给对应车辆 → 回 0x10 时间戳应答到车辆 address:Port。
+    /// 下发 0xB1/0xA1/0x01 等命令亦通过本枢纽单播到车辆 address:Port。
+    /// </summary>
+    internal static class Fass2UdpHub
+    {
+        private static readonly object SyncRoot = new object();
+        private static readonly Dictionary<ushort, IFass2UdpCar> CarsByCode = new Dictionary<ushort, IFass2UdpCar>();
+
+        private static UdpClient _listener;
+        private static Thread _receiveThread;
+        private static volatile bool _running;
+        private static int _listenPort = 20103;
+        private static readonly Dictionary<ushort, long> _lastUnknownCarLogUtcTicks = new Dictionary<ushort, long>();
+
+        public static void Register(IFass2UdpCar car)
+        {
+            if (car == null)
+            {
+                return;
+            }
+
+            lock (SyncRoot)
+            {
+                CarsByCode[car.VehicleCode] = car;
+            }
+        }
+
+        public static void Unregister(IFass2UdpCar car)
+        {
+            if (car == null)
+            {
+                return;
+            }
+
+            Unregister(car.VehicleCode, car);
+        }
+
+        public static void Unregister(ushort vehicleCode, IFass2UdpCar car)
+        {
+            if (car == null)
+            {
+                return;
+            }
+
+            lock (SyncRoot)
+            {
+                if (CarsByCode.TryGetValue(vehicleCode, out var existing) && ReferenceEquals(existing, car))
+                {
+                    CarsByCode.Remove(vehicleCode);
+                }
+            }
+        }
+
+        public static void EnsureStarted(int listenPort)
+        {
+            if (listenPort > 0)
+            {
+                _listenPort = listenPort;
+            }
+
+            lock (SyncRoot)
+            {
+                if (_running)
+                {
+                    return;
+                }
+
+                _listener = new UdpClient(_listenPort);
+                _running = true;
+                _receiveThread = new Thread(ReceiveLoop)
+                {
+                    IsBackground = true,
+                    Name = $"Fass2UdpHub:{_listenPort}"
+                };
+                _receiveThread.Start();
+                Diagnosis.Post($"Fass2UdpHub 已启动，监听 UDP {_listenPort}", "Fass2UdpHub", true);
+            }
+        }
+
+        public static void Stop()
+        {
+            lock (SyncRoot)
+            {
+                _running = false;
+                try
+                {
+                    _listener?.Close();
+                }
+                catch
+                {
+                }
+
+                _listener = null;
+            }
+
+            try
+            {
+                _receiveThread?.Join(1000);
+            }
+            catch
+            {
+            }
+
+            _receiveThread = null;
+        }
+
+        public static void SendToCar(IFass2UdpCar car, byte[] payload)
+        {
+            if (car == null || payload == null || payload.Length == 0)
+            {
+                return;
+            }
+
+            if (string.IsNullOrWhiteSpace(car.RemoteAddress) || car.RemotePort <= 0)
+            {
+                throw new InvalidOperationException(
+                    $"Fass2UdpHub send failed: car VehicleCode={car.VehicleCode} remote endpoint not configured");
+            }
+
+            using var sender = new UdpClient();
+            var remote = new IPEndPoint(IPAddress.Parse(car.RemoteAddress), car.RemotePort);
+            sender.Send(payload, payload.Length, remote);
+        }
+
+        private static void ReceiveLoop()
+        {
+            while (_running)
+            {
+                try
+                {
+                    var remote = new IPEndPoint(IPAddress.Any, 0);
+                    var packet = _listener.Receive(ref remote);
+                    ProcessPacket(packet, remote);
+                }
+                catch (SocketException) when (!_running)
+                {
+                    break;
+                }
+                catch (ObjectDisposedException) when (!_running)
+                {
+                    break;
+                }
+                catch (Exception ex)
+                {
+                    Diagnosis.Post($"Fass2UdpHub receive error: {ex.Message}", "Fass2UdpHub", true);
+                }
+            }
+        }
+
+        private static void ProcessPacket(byte[] packet, IPEndPoint remote)
+        {
+            if (!Fass2Protocol.TryExtractStateFrame(packet, packet.Length, out var frame))
+            {
+                return;
+            }
+
+            Fass2StateReport report;
+            try
+            {
+                report = Fass2Protocol.ParseState(frame);
+            }
+            catch (Exception ex)
+            {
+                Diagnosis.Post($"Fass2UdpHub parse error from {remote}: {ex.Message}", "Fass2UdpHub", true);
+                return;
+            }
+
+            IFass2UdpCar car;
+            lock (SyncRoot)
+            {
+                if (!CarsByCode.TryGetValue(report.Car, out car))
+                {
+                    LogUnknownCar(report.Car);
+                    return;
+                }
+            }
+
+            try
+            {
+                car.OnUdpStateReceived(report);
+            }
+            catch (Exception ex)
+            {
+                Diagnosis.Post($"Fass2UdpHub dispatch error car={report.Car}: {ex.Message}", "Fass2UdpHub", true);
+            }
+
+            try
+            {
+                var ack = Fass2Protocol.BuildStateResponse(report.Car, Fass2Protocol.BeijingUnixTimeMs());
+                SendToCar(car, ack);
+            }
+            catch (Exception ex)
+            {
+                Diagnosis.Post($"Fass2UdpHub 0x10 reply failed car={report.Car}: {ex.Message}", "Fass2UdpHub", true);
+            }
+        }
+
+        private static void LogUnknownCar(ushort carCode)
+        {
+            var nowTicks = DateTime.UtcNow.Ticks;
+            lock (SyncRoot)
+            {
+                if (_lastUnknownCarLogUtcTicks.TryGetValue(carCode, out var lastTicks)
+                    && nowTicks - lastTicks < TimeSpan.FromSeconds(5).Ticks)
+                {
+                    return;
+                }
+
+                _lastUnknownCarLogUtcTicks[carCode] = nowTicks;
+            }
+
+            var registered = string.Join(",", CarsByCode.Keys);
+            if (string.IsNullOrEmpty(registered))
+            {
+                registered = "(无)";
+            }
+
+            Diagnosis.Post(
+                $"Fass2UdpHub 收到 Car={carCode} 的状态，但未注册该编号（已注册: {registered}）。请检查 MagFass2Car.VehicleCode 与模拟器车号是否一致，并重新启动场景。",
+                "Fass2UdpHub",
+                true);
+        }
+    }
+}