350 lines
12 KiB
C#
350 lines
12 KiB
C#
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;
|
|
}
|
|
|
|
if (!HasValidStateChecksum(buffer, i))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
frame = new byte[StateFrameLength];
|
|
Buffer.BlockCopy(buffer, i, frame, 0, StateFrameLength);
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public static bool HasValidStateChecksum(byte[] bytes, int offset = 0)
|
|
{
|
|
if (bytes == null || offset < 0 || offset + StateFrameLength > bytes.Length)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return bytes[offset + 98] == Xor(bytes, offset + 1, 97);
|
|
}
|
|
|
|
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}");
|
|
}
|
|
if (!HasValidStateChecksum(bytes))
|
|
{
|
|
throw new ArgumentException(
|
|
$"invalid FASS2 state xor: got=0x{bytes[98]:X2}, expect=0x{Xor(bytes, 1, 97):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();
|
|
}
|
|
}
|