132 lines
4.9 KiB
C#
132 lines
4.9 KiB
C#
using System;
|
||
|
||
namespace StandardScene.Fass2Simulator
|
||
{
|
||
public sealed class Fass2SimCommandHandler
|
||
{
|
||
private readonly Fass2SimVehicle _vehicle;
|
||
private readonly Fass2SimMotionEngine _motion;
|
||
private readonly Fass2SimActionEngine _actions;
|
||
private readonly bool _logRawFrames;
|
||
private long _stateResponseCount;
|
||
|
||
public Fass2SimCommandHandler(
|
||
Fass2SimVehicle vehicle,
|
||
Fass2SimMotionEngine motion,
|
||
Fass2SimActionEngine actions,
|
||
bool logRawFrames)
|
||
{
|
||
_vehicle = vehicle;
|
||
_motion = motion;
|
||
_actions = actions;
|
||
_logRawFrames = logRawFrames;
|
||
}
|
||
|
||
public void Handle(byte[] packet, string remote)
|
||
{
|
||
if (!Fass2SimProtocol.TryGetCommand(packet, out var command, out var expectedLength))
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] RX 非法报文 from {remote}, len={packet?.Length ?? 0}");
|
||
return;
|
||
}
|
||
|
||
if (packet.Length != expectedLength)
|
||
{
|
||
Fass2SimLog.WriteLine(
|
||
$"[{Now()}] RX 长度不匹配 from {remote}: cmd={Fass2SimProtocol.CommandName(command)} actual={packet.Length} expected={expectedLength}");
|
||
return;
|
||
}
|
||
|
||
if (packet[expectedLength - 1] != Fass2SimProtocol.End)
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] RX 帧尾错误 from {remote}: cmd={Fass2SimProtocol.CommandName(command)}");
|
||
return;
|
||
}
|
||
|
||
_vehicle.RecordCommandReceived();
|
||
_vehicle.LastCommand = command;
|
||
|
||
if (command == Fass2SimProtocol.CmdStateResponse)
|
||
{
|
||
_vehicle.RecordAckReceived();
|
||
LogStateResponseAck(remote);
|
||
return;
|
||
}
|
||
|
||
if (_logRawFrames)
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}: {Fass2SimProtocol.ToHex(packet)}");
|
||
}
|
||
else
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] RX {Fass2SimProtocol.CommandName(command)} from {remote}");
|
||
}
|
||
|
||
switch (command)
|
||
{
|
||
case Fass2SimProtocol.CmdStart:
|
||
_vehicle.State = 1;
|
||
_motion.OnStartCommand();
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
|
||
break;
|
||
case Fass2SimProtocol.CmdStop:
|
||
_vehicle.State = 2;
|
||
_motion.OnStopCommand();
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
|
||
break;
|
||
case Fass2SimProtocol.CmdEmergencyStop:
|
||
_vehicle.State = 3;
|
||
_motion.OnStopCommand();
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> State={Fass2SimProtocol.StateText(_vehicle.State)}");
|
||
break;
|
||
case Fass2SimProtocol.CmdQuery:
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> TCP 查询（UDP 模式通常不发 0x00）");
|
||
break;
|
||
case Fass2SimProtocol.CmdNodes:
|
||
if (Fass2SimProtocol.TryParseNodes(packet, out var taskId, out var nodes))
|
||
{
|
||
_motion.OnNodesCommand(taskId, nodes);
|
||
}
|
||
else
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> 0xB1 解析失败");
|
||
}
|
||
break;
|
||
case Fass2SimProtocol.CmdAction:
|
||
if (Fass2SimProtocol.TryParseAction(packet, out var actionId, out var patch))
|
||
{
|
||
_actions.OnActionCommand(actionId, patch);
|
||
}
|
||
else
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> 0xA1 解析失败");
|
||
}
|
||
break;
|
||
default:
|
||
Fass2SimLog.WriteLine($"[{Now()}] -> 暂未处理");
|
||
break;
|
||
}
|
||
}
|
||
|
||
private void LogStateResponseAck(string remote)
|
||
{
|
||
_stateResponseCount++;
|
||
if (_stateResponseCount == 1)
|
||
{
|
||
Fass2SimLog.WriteLine($"[{Now()}] RX StateResponse(0x10) from {remote}，调度在线（后续 0x10 仅写文件日志）");
|
||
return;
|
||
}
|
||
|
||
if (_stateResponseCount % 200 == 0)
|
||
{
|
||
Fass2SimLog.WriteFileOnly($"[{Now()}] 0x10 累计应答 {_stateResponseCount} 次");
|
||
}
|
||
}
|
||
|
||
private static string Now()
|
||
{
|
||
return DateTime.Now.ToString("HH:mm:ss.fff");
|
||
}
|
||
}
|
||
}
|