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

StandardScene.Magnetic/Protocol/Fass2Protocol.cs

@@ -0,0 +1,336 @@
+using System;
+using System.Linq;
+
+namespace StandardScene.Magnetic.Protocol
+{
+    /// <summary>
+    /// FASS 2.0 通用控制接口（PLC）报文编解码，对齐
+    /// <c>FASS.Extend.Car.Fairyland.Plc</c> 与调度器 <c>CarRequestService</c> 逻辑。
+    /// </summary>
+    internal static class Fass2Protocol
+    {
+        public const byte Begin = 0xBB;
+        public const byte End = 0xEE;
+
+        public const byte CmdQuery = 0x00;
+        public const byte CmdStart = 0x01;
+        public const byte CmdStop = 0x02;
+        public const byte CmdEmergencyStop = 0x03;
+        public const byte CmdReset = 0x04;
+        public const byte CmdRest = 0x05;
+        public const byte CmdShutdown = 0x06;
+        public const byte CmdAction = 0xA1;
+        public const byte CmdNodes = 0xB1;
+        public const byte CmdStateResponse = 0x10;
+
+        public const int ControlFrameLength = 50;
+        public const int StateFrameLength = 100;
+        public const int ActionFrameLength = 100;
+        public const int NodesFrameLength = 300;
+
+        public static byte Xor(byte[] data, int start, int length)
+        {
+            byte xor = 0;
+            for (var i = 0; i < length; i++)
+            {
+                xor ^= data[start + i];
+            }
+            return xor;
+        }
+
+        public static byte[] BuildControl(byte command, ushort car, ushort param)
+        {
+            var frame = new byte[ControlFrameLength];
+            frame[0] = Begin;
+            frame[1] = command;
+            WriteUInt16(frame, 2, car);
+            WriteUInt16(frame, 4, param);
+            frame[48] = Xor(frame, 1, 47);
+            frame[49] = End;
+            return frame;
+        }
+
+        public static byte[] BuildNodes(ushort car, ulong taskId, Fass2NodeMessage[] nodes)
+        {
+            if (nodes == null || nodes.Length == 0)
+            {
+                throw new ArgumentException("nodes required");
+            }
+            if (nodes.Length > 10)
+            {
+                throw new ArgumentException("FASS2 nodes count must be <= 10");
+            }
+
+            var frame = new byte[NodesFrameLength];
+            frame[0] = Begin;
+            frame[1] = CmdNodes;
+            WriteUInt16(frame, 2, car);
+            WriteUInt64(frame, 4, taskId);
+            WriteUInt16(frame, 12, (ushort)nodes.Length);
+            for (var i = 0; i < nodes.Length; i++)
+            {
+                Buffer.BlockCopy(nodes[i].ToBytes(), 0, frame, 14 + i * 25, 25);
+            }
+            frame[298] = Xor(frame, 1, 297);
+            frame[299] = End;
+            return frame;
+        }
+
+        public static byte[] BuildAction(ushort car, ulong actionId, Fass2NodeMessage node)
+        {
+            if (node == null)
+            {
+                throw new ArgumentException("node required");
+            }
+
+            var frame = new byte[ActionFrameLength];
+            frame[0] = Begin;
+            frame[1] = CmdAction;
+            WriteUInt16(frame, 2, car);
+            WriteUInt64(frame, 4, actionId);
+            Buffer.BlockCopy(node.ToBytes(), 0, frame, 14, 25);
+            frame[98] = Xor(frame, 1, 97);
+            frame[99] = End;
+            return frame;
+        }
+
+        /// <summary>
+        /// PCB/UDP 模式：收到 AGV 主动上报的 100B 状态后，回 0x10（50B）时间戳应答。
+        /// </summary>
+        public static byte[] BuildStateResponse(ushort car, ulong timestampMs)
+        {
+            var frame = new byte[ControlFrameLength];
+            frame[0] = Begin;
+            frame[1] = CmdStateResponse;
+            WriteUInt16(frame, 2, car);
+            WriteUInt64(frame, 4, timestampMs);
+
+            var epoch = new DateTime(1970, 1, 1, 8, 0, 0, DateTimeKind.Unspecified);
+            var date = epoch.AddMilliseconds(timestampMs > long.MaxValue ? long.MaxValue : (long)timestampMs);
+            frame[12] = (byte)(date.Year - 1970);
+            frame[13] = (byte)date.Month;
+            frame[14] = (byte)date.Day;
+            frame[15] = (byte)date.Hour;
+            frame[16] = (byte)date.Minute;
+            frame[17] = (byte)date.Second;
+            WriteUInt16(frame, 18, (ushort)date.Millisecond);
+
+            frame[48] = Xor(frame, 1, 47);
+            frame[49] = End;
+            return frame;
+        }
+
+        public static ulong BeijingUnixTimeMs()
+        {
+            var epoch = new DateTime(1970, 1, 1, 8, 0, 0, DateTimeKind.Unspecified);
+            return (ulong)(DateTime.Now - epoch).TotalMilliseconds;
+        }
+
+        public static bool TryExtractStateFrame(byte[] buffer, int length, out byte[] frame)
+        {
+            frame = null;
+            if (buffer == null || length < StateFrameLength)
+            {
+                return false;
+            }
+
+            for (var i = 0; i <= length - StateFrameLength; i++)
+            {
+                if (buffer[i] != Begin || buffer[i + StateFrameLength - 1] != End)
+                {
+                    continue;
+                }
+
+                frame = new byte[StateFrameLength];
+                Buffer.BlockCopy(buffer, i, frame, 0, StateFrameLength);
+                return true;
+            }
+
+            if (length == StateFrameLength && buffer[0] == Begin && buffer[StateFrameLength - 1] == End)
+            {
+                frame = new byte[StateFrameLength];
+                Buffer.BlockCopy(buffer, 0, frame, 0, StateFrameLength);
+                return true;
+            }
+
+            return false;
+        }
+
+        public static Fass2StateReport ParseState(byte[] bytes)
+        {
+            if (bytes == null || bytes.Length < StateFrameLength)
+            {
+                throw new ArgumentException("FASS2 state frame must be 100 bytes");
+            }
+            if (bytes[0] != Begin || bytes[99] != End)
+            {
+                throw new ArgumentException($"invalid FASS2 state frame: begin=0x{bytes[0]:X2}, end=0x{bytes[99]:X2}");
+            }
+
+            var nodeBytes = new byte[25];
+            Buffer.BlockCopy(bytes, 53, nodeBytes, 0, 25);
+            return new Fass2StateReport
+            {
+                Command = bytes[1],
+                Car = ReadUInt16(bytes, 2),
+                Length = ReadUInt16(bytes, 4),
+                Width = ReadUInt16(bytes, 6),
+                BatteryCharge = bytes[28],
+                BatteryHealth = bytes[29],
+                BatteryCurrent = ReadUInt16(bytes, 30),
+                BatteryVoltage = ReadUInt16(bytes, 32),
+                HeadingAngle = ReadUInt16(bytes, 34),
+                State = bytes[36],
+                Alarm = ReadUInt64(bytes, 37),
+                Task = ReadUInt64(bytes, 45),
+                Node = Fass2NodeMessage.FromBytes(nodeBytes)
+            };
+        }
+
+        public static string ToHex(byte[] bytes)
+        {
+            return bytes == null ? string.Empty : string.Join(" ", bytes.Select(b => b.ToString("X2")));
+        }
+
+        public static string StateText(byte state)
+        {
+            switch (state)
+            {
+                case 0: return "未准备";
+                case 1: return "运行中";
+                case 2: return "停止中";
+                case 3: return "急停中";
+                case 4: return "故障中";
+                case 5: return "任务中";
+                case 6: return "休眠中";
+                case 7: return "关机中";
+                case 8: return "充电中";
+                default: return $"未知({state})";
+            }
+        }
+
+        private static void WriteUInt16(byte[] buffer, int offset, ushort value)
+        {
+            var bytes = BitConverter.GetBytes(value);
+            buffer[offset] = bytes[0];
+            buffer[offset + 1] = bytes[1];
+        }
+
+        private static void WriteUInt64(byte[] buffer, int offset, ulong value)
+        {
+            var bytes = BitConverter.GetBytes(value);
+            Buffer.BlockCopy(bytes, 0, buffer, offset, 8);
+        }
+
+        private static ushort ReadUInt16(byte[] bytes, int offset)
+        {
+            return BitConverter.ToUInt16(bytes, offset);
+        }
+
+        private static ulong ReadUInt64(byte[] bytes, int offset)
+        {
+            return BitConverter.ToUInt64(bytes, offset);
+        }
+    }
+
+    public sealed class Fass2NodeMessage
+    {
+        public ushort Node { get; set; }
+        public ushort Distance { get; set; }
+        public byte StartStop { get; set; }
+        public byte Direction { get; set; }
+        public byte Orientation { get; set; }
+        public byte Byroad { get; set; }
+        public ushort Speed { get; set; }
+        public byte Obstacle { get; set; }
+        public byte Audio { get; set; }
+        public byte Light { get; set; }
+        public byte Charge { get; set; }
+        public byte Rest { get; set; }
+        public byte Lift { get; set; }
+        public byte Clamp { get; set; }
+        public byte Tray { get; set; }
+        public byte Roll { get; set; }
+        public byte Shutdown { get; set; }
+
+        public byte[] ToBytes()
+        {
+            var bytes = new byte[25];
+            WriteUInt16(bytes, 0, Node);
+            WriteUInt16(bytes, 2, Distance);
+            bytes[4] = StartStop;
+            bytes[5] = Direction;
+            bytes[6] = Orientation;
+            bytes[7] = Byroad;
+            WriteUInt16(bytes, 8, Speed);
+            bytes[10] = Obstacle;
+            bytes[11] = Audio;
+            bytes[12] = Light;
+            bytes[13] = Charge;
+            bytes[14] = Rest;
+            bytes[15] = Lift;
+            bytes[16] = Clamp;
+            bytes[17] = Tray;
+            bytes[18] = Roll;
+            bytes[19] = Shutdown;
+            return bytes;
+        }
+
+        public static Fass2NodeMessage FromBytes(byte[] bytes)
+        {
+            if (bytes == null || bytes.Length < 25)
+            {
+                throw new ArgumentException("node frame must be 25 bytes");
+            }
+            return new Fass2NodeMessage
+            {
+                Node = ReadUInt16(bytes, 0),
+                Distance = ReadUInt16(bytes, 2),
+                StartStop = bytes[4],
+                Direction = bytes[5],
+                Orientation = bytes[6],
+                Byroad = bytes[7],
+                Speed = ReadUInt16(bytes, 8),
+                Obstacle = bytes[10],
+                Audio = bytes[11],
+                Light = bytes[12],
+                Charge = bytes[13],
+                Rest = bytes[14],
+                Lift = bytes[15],
+                Clamp = bytes[16],
+                Tray = bytes[17],
+                Roll = bytes[18],
+                Shutdown = bytes[19]
+            };
+        }
+
+        private static void WriteUInt16(byte[] buffer, int offset, ushort value)
+        {
+            var bytes = BitConverter.GetBytes(value);
+            buffer[offset] = bytes[0];
+            buffer[offset + 1] = bytes[1];
+        }
+
+        private static ushort ReadUInt16(byte[] bytes, int offset)
+        {
+            return BitConverter.ToUInt16(bytes, offset);
+        }
+    }
+
+    public sealed class Fass2StateReport
+    {
+        public byte Command { get; set; }
+        public ushort Car { get; set; }
+        public ushort Length { get; set; }
+        public ushort Width { get; set; }
+        public byte BatteryCharge { get; set; }
+        public byte BatteryHealth { get; set; }
+        public ushort BatteryCurrent { get; set; }
+        public ushort BatteryVoltage { get; set; }
+        public ushort HeadingAngle { get; set; }
+        public byte State { get; set; }
+        public ulong Alarm { get; set; }
+        public ulong Task { get; set; }
+        public Fass2NodeMessage Node { get; set; } = new Fass2NodeMessage();
+    }
+}