StandardSence/StandardScene.MagCarSimulator/Protocol/MagCarSimProtocol.cs

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; }
    }
}