新增1.0和2.0两种协议车型车型
This commit is contained in:
ykkokluo
@@ -0,0 +1,228 @@
|
||||
using System;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
|
||||
namespace StandardScene.Fass2Simulator
|
||||
{
|
||||
/// <summary>
|
||||
/// FASS 2.0 PCB/UDP 报文编解码，字段布局对齐
|
||||
/// <c>StandardScene.Magnetic.Protocol.Fass2Protocol.ParseState</c> 与 backend PCB 仿真器。
|
||||
/// </summary>
|
||||
public static class Fass2SimProtocol
|
||||
{
|
||||
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[] BuildState(Fass2SimVehicleSnapshot snapshot)
|
||||
{
|
||||
var frame = new byte[StateFrameLength];
|
||||
frame[0] = Begin;
|
||||
frame[1] = snapshot.LastCommand;
|
||||
WriteUInt16(frame, 2, snapshot.Car);
|
||||
WriteUInt16(frame, 4, snapshot.Length);
|
||||
WriteUInt16(frame, 6, snapshot.Width);
|
||||
|
||||
var carType = Encoding.ASCII.GetBytes("Fass2Sim".PadRight(16, '\0'));
|
||||
Buffer.BlockCopy(carType, 0, frame, 12, Math.Min(16, carType.Length));
|
||||
|
||||
frame[28] = snapshot.BatteryCharge;
|
||||
frame[29] = snapshot.BatteryHealth;
|
||||
WriteUInt16(frame, 30, snapshot.BatteryCurrent);
|
||||
WriteUInt16(frame, 32, snapshot.BatteryVoltage);
|
||||
WriteUInt16(frame, 34, snapshot.HeadingAngle);
|
||||
frame[36] = snapshot.State;
|
||||
WriteUInt64(frame, 37, snapshot.Alarm);
|
||||
WriteUInt64(frame, 45, snapshot.Task);
|
||||
|
||||
var nodeBytes = BuildNodeBytes(snapshot);
|
||||
Buffer.BlockCopy(nodeBytes, 0, frame, 53, 25);
|
||||
|
||||
frame[98] = Xor(frame, 1, 97);
|
||||
frame[99] = End;
|
||||
return frame;
|
||||
}
|
||||
|
||||
public static byte[] BuildNodeBytes(Fass2SimVehicleSnapshot snapshot)
|
||||
{
|
||||
var bytes = new byte[25];
|
||||
WriteUInt16(bytes, 0, snapshot.Node);
|
||||
WriteUInt16(bytes, 2, snapshot.Distance);
|
||||
bytes[4] = snapshot.StartStop;
|
||||
bytes[5] = snapshot.Direction;
|
||||
bytes[6] = snapshot.Orientation;
|
||||
bytes[7] = snapshot.Byroad;
|
||||
WriteUInt16(bytes, 8, snapshot.Speed);
|
||||
bytes[10] = snapshot.Obstacle;
|
||||
bytes[11] = snapshot.Audio;
|
||||
bytes[12] = snapshot.Light;
|
||||
bytes[13] = snapshot.Charge;
|
||||
bytes[14] = snapshot.Rest;
|
||||
bytes[15] = snapshot.Lift;
|
||||
bytes[16] = snapshot.Clamp;
|
||||
bytes[17] = snapshot.Tray;
|
||||
bytes[18] = snapshot.Roll;
|
||||
bytes[19] = snapshot.Shutdown;
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static bool TryGetCommand(byte[] packet, out byte command, out int expectedLength)
|
||||
{
|
||||
command = 0;
|
||||
expectedLength = 0;
|
||||
if (packet == null || packet.Length < 2 || packet[0] != Begin)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
command = packet[1];
|
||||
expectedLength = ResolvePacketLength(command);
|
||||
return expectedLength > 0;
|
||||
}
|
||||
|
||||
public static int ResolvePacketLength(byte command)
|
||||
{
|
||||
switch (command)
|
||||
{
|
||||
case CmdQuery:
|
||||
case CmdStart:
|
||||
case CmdStop:
|
||||
case CmdEmergencyStop:
|
||||
case CmdReset:
|
||||
case CmdRest:
|
||||
case CmdShutdown:
|
||||
case CmdStateResponse:
|
||||
return ControlFrameLength;
|
||||
case CmdAction:
|
||||
return ActionFrameLength;
|
||||
case CmdNodes:
|
||||
return NodesFrameLength;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
public static string CommandName(byte command)
|
||||
{
|
||||
switch (command)
|
||||
{
|
||||
case CmdQuery: return "Query(0x00)";
|
||||
case CmdStart: return "Start(0x01)";
|
||||
case CmdStop: return "Stop(0x02)";
|
||||
case CmdEmergencyStop: return "EmergencyStop(0x03)";
|
||||
case CmdReset: return "Reset(0x04)";
|
||||
case CmdRest: return "Rest(0x05)";
|
||||
case CmdShutdown: return "Shutdown(0x06)";
|
||||
case CmdAction: return "Action(0xA1)";
|
||||
case CmdNodes: return "Nodes(0xB1)";
|
||||
case CmdStateResponse: return "StateResponse(0x10)";
|
||||
default: return $"Unknown(0x{command: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})";
|
||||
}
|
||||
}
|
||||
|
||||
public static string ToHex(byte[] bytes)
|
||||
{
|
||||
return bytes == null ? string.Empty : string.Join(" ", bytes.Select(b => b.ToString("X2")));
|
||||
}
|
||||
|
||||
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 bool TryParseNodes(byte[] packet, out ulong taskId, out Fass2SimNodeMessage[] nodes)
|
||||
{
|
||||
taskId = 0;
|
||||
nodes = Array.Empty<Fass2SimNodeMessage>();
|
||||
if (packet == null || packet.Length < NodesFrameLength || packet[1] != CmdNodes)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
taskId = BitConverter.ToUInt64(packet, 4);
|
||||
var count = BitConverter.ToUInt16(packet, 12);
|
||||
if (count == 0 || count > 10)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var requiredLength = 14 + count * 25 + 2;
|
||||
if (packet.Length < requiredLength)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
nodes = new Fass2SimNodeMessage[count];
|
||||
for (var i = 0; i < count; i++)
|
||||
{
|
||||
nodes[i] = Fass2SimNodeMessage.FromBytes(packet, 14 + i * 25);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool TryParseAction(byte[] packet, out ulong actionId, out Fass2SimNodeMessage node)
|
||||
{
|
||||
actionId = 0;
|
||||
node = null;
|
||||
if (packet == null || packet.Length < ActionFrameLength || packet[1] != CmdAction)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
actionId = BitConverter.ToUInt64(packet, 4);
|
||||
node = Fass2SimNodeMessage.FromBytes(packet, 14);
|
||||
return true;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user