新增1.0和2.0两种协议车型车型
This commit is contained in:
ykkokluo
@@ -0,0 +1,319 @@
|
||||
using System;
|
||||
|
||||
namespace StandardScene.Fass2Simulator
|
||||
{
|
||||
/// <summary>
|
||||
/// 按 0xB1 节点序列模拟行驶：距离推进 → 到站更新 Node/字段 → State 切为停止供调度判定到站。
|
||||
/// </summary>
|
||||
public sealed class Fass2SimMotionEngine
|
||||
{
|
||||
private readonly Fass2SimVehicle _vehicle;
|
||||
private readonly Fass2SimConfig _config;
|
||||
private readonly Fass2SimActionEngine _actions;
|
||||
private readonly object _syncRoot = new object();
|
||||
|
||||
private ulong _activeTaskId;
|
||||
private Fass2SimNodeMessage[] _windowNodes = Array.Empty<Fass2SimNodeMessage>();
|
||||
private Fass2SimNodeMessage _segmentTarget;
|
||||
private double _segmentProgress;
|
||||
private double _segmentLength;
|
||||
private bool _segmentActive;
|
||||
|
||||
public Fass2SimMotionEngine(Fass2SimVehicle vehicle, Fass2SimConfig config, Fass2SimActionEngine actions)
|
||||
{
|
||||
_vehicle = vehicle;
|
||||
_config = config;
|
||||
_actions = actions;
|
||||
}
|
||||
|
||||
public void OnNodesCommand(ulong taskId, Fass2SimNodeMessage[] nodes)
|
||||
{
|
||||
if (nodes == null || nodes.Length == 0)
|
||||
{
|
||||
Fass2SimLog.WriteLine($"[{Now()}] -> 0xB1 节点为空，忽略");
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
var taskChanged = _activeTaskId != taskId;
|
||||
_activeTaskId = taskId;
|
||||
_windowNodes = nodes;
|
||||
_vehicle.Task = taskId;
|
||||
|
||||
ApplyCurrentNodeFields(nodes);
|
||||
LogNodes(nodes);
|
||||
|
||||
if (_vehicle.State == 3)
|
||||
{
|
||||
Fass2SimLog.WriteLine($"[{Now()}] -> 急停中，不启动运动");
|
||||
return;
|
||||
}
|
||||
|
||||
if (_segmentActive)
|
||||
{
|
||||
if (taskChanged)
|
||||
{
|
||||
Fass2SimLog.WriteLine($"[{Now()}] -> 新 taskId，重置路段");
|
||||
ResetSegment();
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
TryStartNextSegment();
|
||||
}
|
||||
}
|
||||
|
||||
public void OnStartCommand()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (_vehicle.State == 3)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
TryStartNextSegment();
|
||||
}
|
||||
}
|
||||
|
||||
public void OnStopCommand()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
ResetSegment();
|
||||
}
|
||||
}
|
||||
|
||||
public void OnStationActionsCompleted()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
TryResumeFromStation();
|
||||
}
|
||||
}
|
||||
|
||||
public void Tick(int deltaMs)
|
||||
{
|
||||
if (deltaMs <= 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!_segmentActive || _segmentTarget == null || _vehicle.State != 1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var speed = ResolveSpeed(_segmentTarget);
|
||||
var deltaDistance = speed * deltaMs / 1000.0;
|
||||
_segmentProgress += deltaDistance;
|
||||
_vehicle.Distance = (ushort)Math.Min(ushort.MaxValue, Math.Round(_segmentProgress));
|
||||
|
||||
if (_segmentProgress < _segmentLength)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
ArriveAtTarget();
|
||||
}
|
||||
}
|
||||
|
||||
private void TryStartNextSegment()
|
||||
{
|
||||
if (_vehicle.State != 1 && _vehicle.State != 2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_vehicle.State == 2 && !_actions.CanLeaveStation())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var target = ResolveNextTarget(_windowNodes, _vehicle.Node);
|
||||
if (target == null)
|
||||
{
|
||||
if (_vehicle.State == 2)
|
||||
{
|
||||
Fass2SimLog.WriteLine($"[{Now()}] -> 窗口内无下一段，等待下一帧 0xB1");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
_segmentTarget = target;
|
||||
_segmentLength = ResolveSegmentLength(target);
|
||||
_segmentProgress = 0;
|
||||
_segmentActive = true;
|
||||
_vehicle.State = 1;
|
||||
_vehicle.Distance = 0;
|
||||
|
||||
if (target.Speed > 0)
|
||||
{
|
||||
_vehicle.Speed = target.Speed;
|
||||
}
|
||||
|
||||
Fass2SimLog.WriteLine(
|
||||
$"[{Now()}] -> 开始路段: {_vehicle.Node} -> {target.Node}, dist={_segmentLength:F0}mm, speed={ResolveSpeed(target):F0}mm/s");
|
||||
}
|
||||
|
||||
private void ArriveAtTarget()
|
||||
{
|
||||
var target = _segmentTarget;
|
||||
_vehicle.Node = target.Node;
|
||||
_vehicle.Distance = 0;
|
||||
ResetSegment();
|
||||
_actions.OnStationArrival(target);
|
||||
|
||||
Fass2SimLog.WriteLine(
|
||||
$"[{Now()}] -> 到站 node={target.Node}, StartStop={target.StartStop}, state={Fass2SimProtocol.StateText(_vehicle.State)}");
|
||||
|
||||
TryResumeFromStation();
|
||||
}
|
||||
|
||||
private void TryResumeFromStation()
|
||||
{
|
||||
if (!_config.AutoContinueOnPass)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var expected = _actions.ExpectedStation;
|
||||
if (expected == null || expected.StartStop != Fass2SimActionResolver.StartStopPass)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (_vehicle.State != 2 || !_actions.CanLeaveStation() || !HasNextTarget(_vehicle.Node))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_vehicle.State = 1;
|
||||
TryStartNextSegment();
|
||||
}
|
||||
|
||||
private void ResetSegment()
|
||||
{
|
||||
_segmentTarget = null;
|
||||
_segmentProgress = 0;
|
||||
_segmentLength = 0;
|
||||
_segmentActive = false;
|
||||
}
|
||||
|
||||
private bool HasNextTarget(ushort currentNode)
|
||||
{
|
||||
return ResolveNextTarget(_windowNodes, currentNode) != null;
|
||||
}
|
||||
|
||||
private static Fass2SimNodeMessage ResolveNextTarget(Fass2SimNodeMessage[] nodes, ushort currentNode)
|
||||
{
|
||||
if (nodes == null || nodes.Length == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var startIndex = 0;
|
||||
for (var i = 0; i < nodes.Length; i++)
|
||||
{
|
||||
if (nodes[i].Node == currentNode)
|
||||
{
|
||||
startIndex = i + 1;
|
||||
}
|
||||
}
|
||||
|
||||
for (var i = startIndex; i < nodes.Length; i++)
|
||||
{
|
||||
var node = nodes[i];
|
||||
if (node.Node == 0 || node.Node == currentNode)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private void ApplyCurrentNodeFields(Fass2SimNodeMessage[] nodes)
|
||||
{
|
||||
foreach (var node in nodes)
|
||||
{
|
||||
if (node.Node == _vehicle.Node && node.StartStop > 0)
|
||||
{
|
||||
_vehicle.StartStop = node.StartStop;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private double ResolveSegmentLength(Fass2SimNodeMessage target)
|
||||
{
|
||||
if (target.Distance > 0)
|
||||
{
|
||||
return target.Distance;
|
||||
}
|
||||
|
||||
if (_config.DefaultSegmentDistance > 0)
|
||||
{
|
||||
return _config.DefaultSegmentDistance;
|
||||
}
|
||||
|
||||
var speed = ResolveSpeed(target);
|
||||
return Math.Max(100, speed * _config.SecondsPerSegment);
|
||||
}
|
||||
|
||||
private double ResolveSpeed(Fass2SimNodeMessage target)
|
||||
{
|
||||
if (target.Speed > 0)
|
||||
{
|
||||
return target.Speed;
|
||||
}
|
||||
|
||||
if (_vehicle.Speed > 0)
|
||||
{
|
||||
return _vehicle.Speed;
|
||||
}
|
||||
|
||||
return _config.DefaultSpeed;
|
||||
}
|
||||
|
||||
private void LogNodes(Fass2SimNodeMessage[] nodes)
|
||||
{
|
||||
var summary = string.Join(" -> ", Array.ConvertAll(nodes, n => n.Node.ToString()));
|
||||
Fass2SimLog.WriteLine($"[{Now()}] -> 0xB1 taskId={_activeTaskId}, nodes=[{summary}]");
|
||||
}
|
||||
|
||||
private static string Now()
|
||||
{
|
||||
return DateTime.Now.ToString("HH:mm:ss.fff");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 供界面展示路段进度：行驶中返回目标节点与路段长度，到站后 node 才会在协议里跳变。
|
||||
/// </summary>
|
||||
public bool TryGetSegmentStatus(out ushort targetNode, out int segmentLengthMm, out int progressMm)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!_segmentActive || _segmentTarget == null)
|
||||
{
|
||||
targetNode = 0;
|
||||
segmentLengthMm = 0;
|
||||
progressMm = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
targetNode = _segmentTarget.Node;
|
||||
segmentLengthMm = (int)Math.Round(_segmentLength);
|
||||
progressMm = (int)Math.Round(_segmentProgress);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user