磁导航1.0内部交管和信号交互

2026-08-24 14:55:35 +08:00
parent cacc5fea5c
commit 244b0af6c8
60 changed files with 6395 additions and 294 deletions
@@ -0,0 +1,144 @@
+using System;
+
+namespace StandardScene.MagCarSimulator
+{
+    /// <summary>
+    /// FASS 1.0 32 字节帧，字段布局与 <c>MagCar.MagCarReport</c> 对齐。
+    /// </summary>
+    public static class MagCarSimProtocol
+    {
+        public const int FrameLength = 32;
+        public const byte Begin = 0xBB;
+        public const byte End = 0xEE;
+
+        public const byte CmdQueryState = 0x00;
+        public const byte CmdStart = 0x01;
+        public const byte CmdStop = 0x02;
+        public const byte CmdSpeed = 0x03;
+        public const byte CmdAngle = 0x04;
+        public const byte CmdLegacyNode = 0x05;
+        public const byte CmdTask = 0xA1;
+
+        public const byte StateIdle = 0;
+        public const byte StateRunning = 1;
+        public const byte StateStopping = 2;
+        public const byte StateCharging = 3;
+
+        public static bool TryParse(byte[] bytes, out MagCarSimRequest request)
+        {
+            request = null;
+            if (bytes == null || bytes.Length < FrameLength)
+            {
+                return false;
+            }
+
+            if (bytes[0] != Begin || bytes[FrameLength - 1] != End)
+            {
+                return false;
+            }
+
+            request = new MagCarSimRequest
+            {
+                Command = bytes[1],
+                VehicleCode = ReadUInt16(bytes, 2),
+                Data0 = ReadUInt16(bytes, 4),
+                Data1 = ReadUInt16(bytes, 6)
+            };
+            return true;
+        }
+
+        public static byte[] BuildStatus(
+            byte command,
+            ushort vehicleCode,
+            ushort node,
+            byte state,
+            byte charge,
+            float current,
+            float voltage,
+            byte speed,
+            ushort angle,
+            byte task,
+            byte lift,
+            byte roll)
+        {
+            var bytes = new byte[FrameLength];
+            bytes[0] = Begin;
+            bytes[1] = command;
+            WriteUInt16(bytes, 2, vehicleCode);
+            WriteUInt16(bytes, 4, node);
+            WriteUInt16(bytes, 25, angle);
+            WriteSingle(bytes, 16, current);
+            WriteSingle(bytes, 20, voltage);
+            bytes[14] = state;
+            bytes[15] = charge;
+            bytes[24] = speed;
+            bytes[27] = task;
+            bytes[28] = lift;
+            bytes[29] = roll;
+            bytes[31] = End;
+            return bytes;
+        }
+
+        public static string CommandName(byte command)
+        {
+            switch (command)
+            {
+                case CmdQueryState: return "Query(0x00)";
+                case CmdStart: return "Start(0x01)";
+                case CmdStop: return "Stop(0x02)";
+                case CmdSpeed: return "Speed(0x03)";
+                case CmdAngle: return "Angle(0x04)";
+                case CmdLegacyNode: return "LegacyNode(0x05)";
+                case CmdTask: return "Task(0xA1)";
+                default: return $"Unknown(0x{command:X2})";
+            }
+        }
+
+        public static string StateText(byte state)
+        {
+            switch (state)
+            {
+                case StateIdle: return "未准备";
+                case StateRunning: return "运行中";
+                case StateStopping: return "停止中";
+                case StateCharging: return "充电中";
+                default: return $"未知({state})";
+            }
+        }
+
+        public static string ToHex(byte[] bytes)
+        {
+            return bytes == null ? string.Empty : BitConverter.ToString(bytes).Replace("-", " ");
+        }
+
+        public static ushort ReadUInt16(byte[] bytes, int offset)
+        {
+            return (ushort)((bytes[offset] << 8) | bytes[offset + 1]);
+        }
+
+        public static void WriteUInt16(byte[] bytes, int offset, ushort value)
+        {
+            bytes[offset] = (byte)(value >> 8);
+            bytes[offset + 1] = (byte)(value & 0xFF);
+        }
+
+        public static void WriteSingle(byte[] bytes, int offset, float value)
+        {
+            var raw = BitConverter.GetBytes(value);
+            if (BitConverter.IsLittleEndian)
+            {
+                Array.Reverse(raw);
+            }
+
+            Buffer.BlockCopy(raw, 0, bytes, offset, 4);
+        }
+    }
+
+    public sealed class MagCarSimRequest
+    {
+        public byte Command { get; set; }
+        public ushort VehicleCode { get; set; }
+        public ushort Data0 { get; set; }
+        public ushort Data1 { get; set; }
+    }
+}