diff --git a/Directory.Build.props b/Directory.Build.props index 76682cb..4e595db 100644 --- a/Directory.Build.props +++ b/Directory.Build.props @@ -1,5 +1,11 @@ + + $([System.IO.Path]::GetFullPath('$(MSBuildThisFileDirectory)..\..\stand\')) + $(StandRoot)ref\ + $(StandRoot)SimpleLite\ D:\MDCS\Dependencies\Commons + $(MSBuildThisFileDirectory)StandardScene.Core\Ref\leegKeys-sdk.dll + $(StandSimpleLiteDir)plugins\leegKeys-sdk.dll diff --git a/StandardScene.Core/Chained/AbstractLoopMission.cs b/StandardScene.Core/Chained/AbstractLoopMission.cs index 2c9f6e6..983fc0e 100644 --- a/StandardScene.Core/Chained/AbstractLoopMission.cs +++ b/StandardScene.Core/Chained/AbstractLoopMission.cs @@ -1058,6 +1058,22 @@ namespace StandardScene.Chained } catch (Exception ex) { + try + { + car.tags.Remove("occupied"); + if (car is Car trafficCar && car.GetLastSite() > 0) + { + var currentSite = SimpleLib.GetSite(car.GetLastSite()); + if (currentSite != null) + { + trafficCar.TrafficReset(currentSite, true, strict: false); + } + } + } + catch + { + } + // 记录异常并等待后重试 Diagnosis.Post( $"{car.name}循环任务报错{ExceptionFormatter.FormatEx(ex)}--去往循环点id:{targetSite.id},name:{targetSite.name}", diff --git a/StandardScene.Core/StandardScene.Core.csproj b/StandardScene.Core/StandardScene.Core.csproj index c880e34..f7b88c0 100644 --- a/StandardScene.Core/StandardScene.Core.csproj +++ b/StandardScene.Core/StandardScene.Core.csproj @@ -25,32 +25,31 @@ - - E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\SimpleLite.dll + + $(StandRefDir)CommonUsage.dll + + + $(StandRefDir)CycleGUI.dll + + + $(StandRefDir)LessokajiWeaverUtilities.dll + + + $(StandRefDir)MDCSToolBox.dll + + + $(StandRefDir)SimpleCore.dll - - $(CGUILibDir)\CycleGUI.dll - false + + $(LeegKeysSdkPath) - - E:\Work\Core\Simple-FR\Simple\SimpleCore\bin\Debug\netstandard2.0\SimpleCore.dll - - - D:\MDCS\Dependencies\Commons\CommonUsage.dll - - - D:\MDCS\Dependencies\Commons\MDCSToolBox.dll + + $(StandSimpleLiteDir)SimpleLite.dll - E:\Work\Core\Simple-FR\Simple\tools\Topaz.dll - - - E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\LessokajiWeaverUtilities.dll - - - Ref\leegKeys-sdk.dll + $(StandRefDir)Topaz.dll diff --git a/StandardScene.Devices/StandardScene.Devices.csproj b/StandardScene.Devices/StandardScene.Devices.csproj index b81d123..9e53f0a 100644 --- a/StandardScene.Devices/StandardScene.Devices.csproj +++ b/StandardScene.Devices/StandardScene.Devices.csproj @@ -30,23 +30,23 @@ - - E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\SimpleLite.dll - - - E:\Work\Core\Simple-FR\Simple\SimpleCore\bin\Debug\netstandard2.0\SimpleCore.dll - - D:\MDCS\Dependencies\Commons\CommonUsage.dll - - - E:\Work\Core\Simple-FR\Simple\tools\Topaz.dll - - - E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\LessokajiWeaverUtilities.dll + $(StandRefDir)CommonUsage.dll - ..\StandardScene.Core\Ref\leegKeys-sdk.dll + $(LeegKeysSdkPath) + + + $(StandRefDir)LessokajiWeaverUtilities.dll + + + $(StandRefDir)SimpleCore.dll + + + $(StandSimpleLiteDir)SimpleLite.dll + + + $(StandRefDir)Topaz.dll diff --git a/StandardScene.Fass2Simulator/App.xaml b/StandardScene.Fass2Simulator/App.xaml new file mode 100644 index 0000000..4d06091 --- /dev/null +++ b/StandardScene.Fass2Simulator/App.xaml @@ -0,0 +1,9 @@ + + + + + + + diff --git a/StandardScene.Fass2Simulator/App.xaml.cs b/StandardScene.Fass2Simulator/App.xaml.cs new file mode 100644 index 0000000..895d57f --- /dev/null +++ b/StandardScene.Fass2Simulator/App.xaml.cs @@ -0,0 +1,43 @@ +using System; +using System.Windows; + +namespace StandardScene.Fass2Simulator +{ + public partial class App : Application + { + protected override void OnStartup(StartupEventArgs e) + { + if (HasTestArg(e.Args)) + { + var code = Fass2SimSelfTests.Run(e.Args); + Shutdown(code); + return; + } + + base.OnStartup(e); + Exit += (_, __) => Fass2SimFileLogger.Shutdown(); + var window = new MainWindow(); + MainWindow = window; + window.Show(); + } + + private static bool HasTestArg(string[] args) + { + if (args == null) + { + return false; + } + + foreach (var arg in args) + { + if (string.Equals(arg, "--motion-test", StringComparison.OrdinalIgnoreCase) + || string.Equals(arg, "--action-test", StringComparison.OrdinalIgnoreCase)) + { + return true; + } + } + + return false; + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs b/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs new file mode 100644 index 0000000..581bbb2 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimActionEngine.cs @@ -0,0 +1,351 @@ +using System; +using System.Collections.Generic; + +namespace StandardScene.Fass2Simulator +{ + /// + /// 到站动作闭环：0xB1 登记期望 → 0xA1 或定时器补全字段 → 供调度 AtStation 判定完成。 + /// + public sealed class Fass2SimActionEngine + { + private readonly Fass2SimVehicle _vehicle; + private readonly Fass2SimConfig _config; + private readonly Fass2SimNodeProfileStore _profiles; + private readonly object _syncRoot = new object(); + private readonly List _pendingActions = new List(); + + private Fass2SimNodeMessage _expectedStation; + + public Fass2SimActionEngine(Fass2SimVehicle vehicle, Fass2SimConfig config, Fass2SimNodeProfileStore profiles) + { + _vehicle = vehicle; + _config = config; + _profiles = profiles ?? new Fass2SimNodeProfileStore(); + } + + public event Action StationActionsCompleted; + + public void OnStationArrival(Fass2SimNodeMessage station) + { + if (station == null) + { + return; + } + + lock (_syncRoot) + { + _pendingActions.Clear(); + _expectedStation = Clone(station); + + ApplyArrivalFields(station); + _vehicle.State = 2; + + var pending = Fass2SimActionResolver.DescribePending(_expectedStation, _vehicle.CopySnapshotUnsafe()); + if (string.IsNullOrEmpty(pending)) + { + Fass2SimLog.WriteLine($"[{Now()}] -> 站点 node={station.Node} 无待执行动作"); + NotifyStationCompleteIfReady(); + return; + } + + Fass2SimLog.WriteLine($"[{Now()}] -> 站点 node={station.Node} 待动作: {pending}"); + + if (_config.AutoCompleteStationActions) + { + SchedulePatch(Clone(station), 0, 0, ResolveActionDelayMs(station.Node)); + Fass2SimLog.WriteLine($"[{Now()}] -> AutoComplete 已排队，delay={ResolveActionDelayMs(station.Node)}ms"); + } + } + } + + public void OnActionCommand(ulong actionId, Fass2SimNodeMessage patch) + { + if (patch == null) + { + return; + } + + lock (_syncRoot) + { + if (patch.Node != 0 && patch.Node != _vehicle.Node) + { + Fass2SimLog.WriteLine( + $"[{Now()}] -> 0xA1 节点不匹配: patch={patch.Node}, current={_vehicle.Node}，仍尝试应用"); + } + + var delay = ResolveActionDelayMs(patch.Node != 0 ? patch.Node : _vehicle.Node); + SchedulePatch(patch, actionId, 0, delay); + Fass2SimLog.WriteLine( + $"[{Now()}] -> 0xA1 actionId={actionId} 已排队，delay={delay}ms, pending={DescribePatch(patch)}"); + } + } + + public void Tick(int deltaMs) + { + if (deltaMs <= 0) + { + return; + } + + List due = null; + lock (_syncRoot) + { + for (var i = _pendingActions.Count - 1; i >= 0; i--) + { + _pendingActions[i].RemainingMs -= deltaMs; + if (_pendingActions[i].RemainingMs <= 0) + { + due ??= new List(); + due.Add(_pendingActions[i]); + _pendingActions.RemoveAt(i); + } + } + } + + if (due == null) + { + return; + } + + foreach (var action in due) + { + lock (_syncRoot) + { + ApplyPatch(action.Patch); + if (action.ActionId > 0) + { + Fass2SimLog.WriteLine( + $"[{Now()}] -> 动作完成 actionId={action.ActionId}, node={_vehicle.Node}, fields={DescribePatch(action.Patch)}"); + } + else + { + Fass2SimLog.WriteLine( + $"[{Now()}] -> 自动动作完成 node={_vehicle.Node}, fields={DescribePatch(action.Patch)}"); + } + + var pending = Fass2SimActionResolver.DescribePending(_expectedStation, _vehicle.CopySnapshotUnsafe()); + if (!string.IsNullOrEmpty(pending)) + { + Fass2SimLog.WriteLine($"[{Now()}] -> 仍有待动作: {pending}"); + } + + NotifyStationCompleteIfReady(); + } + } + } + + public bool IsStationComplete() + { + lock (_syncRoot) + { + return IsStationCompleteLocked(); + } + } + + public bool CanLeaveStation() + { + lock (_syncRoot) + { + return IsStationCompleteLocked(); + } + } + + public Fass2SimNodeMessage ExpectedStation + { + get + { + lock (_syncRoot) + { + return _expectedStation; + } + } + } + + private bool IsStationCompleteLocked() + { + if (_pendingActions.Count > 0) + { + return false; + } + + if (_expectedStation == null) + { + return true; + } + + return Fass2SimActionResolver.IsStationActionComplete( + _expectedStation, + _vehicle.CopySnapshotUnsafe(), + _vehicle.State); + } + + private void NotifyStationCompleteIfReady() + { + if (!IsStationCompleteLocked()) + { + return; + } + + if (_expectedStation != null) + { + Fass2SimLog.WriteLine($"[{Now()}] -> 站点 node={_expectedStation.Node} 动作已全部完成"); + } + + StationActionsCompleted?.Invoke(); + } + + private void ApplyArrivalFields(Fass2SimNodeMessage station) + { + if (station.StartStop > 0) + { + _vehicle.StartStop = station.StartStop; + } + + if (station.Orientation > 0) + { + _vehicle.Orientation = station.Orientation; + } + + if (station.Direction > 0) + { + _vehicle.Direction = station.Direction; + } + + if (station.Byroad > 0) + { + _vehicle.Byroad = station.Byroad; + } + } + + private void SchedulePatch(Fass2SimNodeMessage patch, ulong actionId, int source, int delayMs) + { + _pendingActions.Add(new PendingAction + { + Patch = Clone(patch), + ActionId = actionId, + RemainingMs = Math.Max(0, delayMs) + }); + } + + private void ApplyPatch(Fass2SimNodeMessage patch) + { + if (patch.Node != 0) + { + _vehicle.Node = patch.Node; + } + + if (patch.StartStop > 0) + { + _vehicle.StartStop = patch.StartStop; + } + if (patch.Speed > 0) + { + _vehicle.Speed = patch.Speed; + } + if (patch.Direction > 0) + { + _vehicle.Direction = patch.Direction; + } + if (patch.Orientation > 0) + { + _vehicle.Orientation = patch.Orientation; + } + if (patch.Byroad > 0) + { + _vehicle.Byroad = patch.Byroad; + } + if (patch.Obstacle > 0) + { + _vehicle.Obstacle = patch.Obstacle; + } + if (patch.Audio > 0) + { + _vehicle.Audio = patch.Audio; + } + if (patch.Light > 0) + { + _vehicle.Light = patch.Light; + } + if (patch.Charge > 0) + { + _vehicle.Charge = patch.Charge; + } + if (patch.Rest > 0) + { + _vehicle.Rest = patch.Rest; + } + if (patch.Lift > 0) + { + _vehicle.Lift = patch.Lift; + } + if (patch.Clamp > 0) + { + _vehicle.Clamp = patch.Clamp; + } + if (patch.Tray > 0) + { + _vehicle.Tray = patch.Tray; + } + if (patch.Roll > 0) + { + _vehicle.Roll = patch.Roll; + } + if (patch.Shutdown > 0) + { + _vehicle.Shutdown = patch.Shutdown; + } + } + + private int ResolveActionDelayMs(ushort nodeId) + { + return _profiles.ResolveActionDelayMs(nodeId, _config.ActionDelayMs); + } + + private static Fass2SimNodeMessage Clone(Fass2SimNodeMessage source) + { + return new Fass2SimNodeMessage + { + Node = source.Node, + Distance = source.Distance, + StartStop = source.StartStop, + Direction = source.Direction, + Orientation = source.Orientation, + Byroad = source.Byroad, + Speed = source.Speed, + Obstacle = source.Obstacle, + Audio = source.Audio, + Light = source.Light, + Charge = source.Charge, + Rest = source.Rest, + Lift = source.Lift, + Clamp = source.Clamp, + Tray = source.Tray, + Roll = source.Roll, + Shutdown = source.Shutdown + }; + } + + private static string DescribePatch(Fass2SimNodeMessage patch) + { + var parts = new List(); + if (patch.StartStop > 0) parts.Add($"StartStop={patch.StartStop}"); + if (patch.Lift > 0) parts.Add($"Lift={patch.Lift}"); + if (patch.Charge > 0) parts.Add($"Charge={patch.Charge}"); + if (patch.Roll > 0) parts.Add($"Roll={patch.Roll}"); + if (patch.Speed > 0) parts.Add($"Speed={patch.Speed}"); + return parts.Count == 0 ? "(empty)" : string.Join(",", parts); + } + + private static string Now() + { + return DateTime.Now.ToString("HH:mm:ss.fff"); + } + + private sealed class PendingAction + { + public Fass2SimNodeMessage Patch { get; set; } + public ulong ActionId { get; set; } + public int RemainingMs { get; set; } + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs b/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs new file mode 100644 index 0000000..6658a2a --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimActionResolver.cs @@ -0,0 +1,142 @@ +using System; +using System.Collections.Generic; + +namespace StandardScene.Fass2Simulator +{ + /// + /// 对齐 Fass2ActionResolver 的到站动作判定（独立副本，避免引用 Magnetic）。 + /// + public static class Fass2SimActionResolver + { + public const byte StartStopPass = 1; + public const byte StartStopStop = 2; + public const byte StartStopPrecision = 22; + + private static readonly (string Name, Func Get)[] ActionFields = + { + ("Direction", n => n.Direction), + ("Orientation", n => n.Orientation), + ("Byroad", n => n.Byroad), + ("Obstacle", n => n.Obstacle), + ("Audio", n => n.Audio), + ("Light", n => n.Light), + ("Charge", n => n.Charge), + ("Rest", n => n.Rest), + ("Lift", n => n.Lift), + ("Clamp", n => n.Clamp), + ("Tray", n => n.Tray), + ("Roll", n => n.Roll), + ("Shutdown", n => n.Shutdown) + }; + + public static bool RequiresActionWait(Fass2SimNodeMessage expected) + { + if (expected == null) + { + return false; + } + + if (expected.StartStop is StartStopStop or StartStopPrecision) + { + return true; + } + + foreach (var field in ActionFields) + { + if (field.Get(expected) != 0) + { + return true; + } + } + + return expected.Speed != 0; + } + + public static bool IsStationActionComplete(Fass2SimNodeMessage expected, Fass2SimVehicleSnapshot actual, byte vehicleState) + { + if (expected == null || actual == null) + { + return false; + } + + if (actual.Node != expected.Node) + { + return false; + } + + if (vehicleState == 1) + { + return false; + } + + if (!RequiresActionWait(expected)) + { + return true; + } + + if (expected.StartStop == StartStopPass) + { + return true; + } + + return ListPendingFields(expected, actual).Count == 0; + } + + public static List ListPendingFields(Fass2SimNodeMessage expected, Fass2SimVehicleSnapshot actual) + { + var pending = new List(); + if (expected == null || actual == null) + { + return pending; + } + + if (expected.StartStop != 0 && actual.StartStop != expected.StartStop) + { + pending.Add("StartStop"); + } + + if (expected.Speed != 0 && actual.Speed != expected.Speed) + { + pending.Add("Speed"); + } + + foreach (var field in ActionFields) + { + var expectedValue = field.Get(expected); + if (expectedValue != 0 && GetActualField(actual, field.Name) != expectedValue) + { + pending.Add(field.Name); + } + } + + return pending; + } + + public static string DescribePending(Fass2SimNodeMessage expected, Fass2SimVehicleSnapshot actual) + { + var pending = ListPendingFields(expected, actual); + return pending.Count == 0 ? string.Empty : string.Join(",", pending); + } + + private static byte GetActualField(Fass2SimVehicleSnapshot actual, string name) + { + switch (name) + { + case "Direction": return actual.Direction; + case "Orientation": return actual.Orientation; + case "Byroad": return actual.Byroad; + case "Obstacle": return actual.Obstacle; + case "Audio": return actual.Audio; + case "Light": return actual.Light; + case "Charge": return actual.Charge; + case "Rest": return actual.Rest; + case "Lift": return actual.Lift; + case "Clamp": return actual.Clamp; + case "Tray": return actual.Tray; + case "Roll": return actual.Roll; + case "Shutdown": return actual.Shutdown; + default: return 0; + } + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs b/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs new file mode 100644 index 0000000..2d7fe84 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimBootstrap.cs @@ -0,0 +1,77 @@ +using System; +using System.Collections.Generic; +using System.IO; +using Microsoft.Extensions.Configuration; +using Newtonsoft.Json; + +namespace StandardScene.Fass2Simulator +{ + public static class Fass2SimBootstrap + { + public static Fass2SimConfig LoadConfig() + { + var basePath = AppContext.BaseDirectory; + var builder = new ConfigurationBuilder() + .SetBasePath(basePath) + .AddJsonFile("appsettings.json", optional: true, reloadOnChange: false); + + var configuration = builder.Build(); + var config = new Fass2SimConfig(); + configuration.GetSection("Fass2Simulator").Bind(config); + return config; + } + + public static Fass2SimNodeProfileStore LoadNodeProfiles(Fass2SimConfig config) + { + var store = new Fass2SimNodeProfileStore(); + var path = ResolveNodeProfilesPath(config.NodeProfilesPath); + if (!File.Exists(path)) + { + return store; + } + + try + { + var json = File.ReadAllText(path); + var profiles = JsonConvert.DeserializeObject>(json) + ?? new Dictionary(); + store.Load(profiles); + Fass2SimLog.WriteLine($"[{DateTime.Now:HH:mm:ss}] 已加载节点配置: {path} ({profiles.Count} 项)"); + } + catch (Exception ex) + { + Fass2SimLog.WriteLine($"[{DateTime.Now:HH:mm:ss}] 加载节点配置失败: {ex.Message}"); + } + + return store; + } + + public static Fass2SimRuntime CreateRuntime(Fass2SimConfig config) + { + var profiles = LoadNodeProfiles(config); + return new Fass2SimRuntime(config, profiles); + } + + public static (Fass2SimVehicle vehicle, Fass2SimMotionEngine motion, Fass2SimActionEngine actions) CreateTestStack( + Fass2SimConfig config) + { + var vehicle = new Fass2SimVehicle(config); + var actions = new Fass2SimActionEngine(vehicle, config, new Fass2SimNodeProfileStore()); + var motion = new Fass2SimMotionEngine(vehicle, config, actions); + actions.StationActionsCompleted += motion.OnStationActionsCompleted; + return (vehicle, motion, actions); + } + + private static string ResolveNodeProfilesPath(string configuredPath) + { + if (string.IsNullOrWhiteSpace(configuredPath)) + { + return Path.Combine(AppContext.BaseDirectory, "sim-nodes.json"); + } + + return Path.IsPathRooted(configuredPath) + ? configuredPath + : Path.Combine(AppContext.BaseDirectory, configuredPath); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimCommandHandler.cs b/StandardScene.Fass2Simulator/Fass2SimCommandHandler.cs new file mode 100644 index 0000000..29ea9aa --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimCommandHandler.cs @@ -0,0 +1,131 @@ +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"); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimConfig.cs b/StandardScene.Fass2Simulator/Fass2SimConfig.cs new file mode 100644 index 0000000..e21b3fc --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimConfig.cs @@ -0,0 +1,30 @@ +using System.Collections.Generic; + +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimConfig + { + public ushort VehicleCode { get; set; } = 1; + public string VehicleListenAddress { get; set; } = "0.0.0.0"; + public int VehicleListenPort { get; set; } = 5000; + public string SchedulerHost { get; set; } = "127.0.0.1"; + public int SchedulerListenPort { get; set; } = 20103; + public int ReportIntervalMs { get; set; } = 200; + public ushort InitialNode { get; set; } = 1; + public byte InitialState { get; set; } = 0; + public byte BatteryCharge { get; set; } = 100; + public ushort CarLength { get; set; } = 1200; + public ushort CarWidth { get; set; } = 800; + public bool LogRawFrames { get; set; } + public double DefaultSpeed { get; set; } = 500; + public double DefaultSegmentDistance { get; set; } = 1000; + public double SecondsPerSegment { get; set; } = 2; + public bool AutoContinueOnPass { get; set; } = true; + public int ActionDelayMs { get; set; } = 1000; + public bool AutoCompleteStationActions { get; set; } + public string NodeProfilesPath { get; set; } = "sim-nodes.json"; + public bool EnableFileLog { get; set; } = true; + public string LogDirectory { get; set; } = "logs"; + public Dictionary NodeProfiles { get; set; } = new Dictionary(); + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimFileLogger.cs b/StandardScene.Fass2Simulator/Fass2SimFileLogger.cs new file mode 100644 index 0000000..fb553d3 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimFileLogger.cs @@ -0,0 +1,140 @@ +using System; +using System.IO; +using System.Text; + +namespace StandardScene.Fass2Simulator +{ + public static class Fass2SimFileLogger + { + private static readonly object SyncRoot = new object(); + private static StreamWriter _writer; + private static bool _enabled = true; + private static string _logDirectory; + + public static string CurrentFilePath { get; private set; } + + public static void Configure(Fass2SimConfig config) + { + _enabled = config == null || config.EnableFileLog; + _logDirectory = ResolveLogDirectory(config?.LogDirectory); + } + + public static void BeginSession(Fass2SimConfig config) + { + if (config == null || !config.EnableFileLog) + { + return; + } + + lock (SyncRoot) + { + CloseWriter(); + Directory.CreateDirectory(_logDirectory); + var fileName = $"car{config.VehicleCode}_{DateTime.Now:yyyyMMdd_HHmmss}.log"; + CurrentFilePath = Path.Combine(_logDirectory, fileName); + _writer = new StreamWriter(CurrentFilePath, false, new UTF8Encoding(encoderShouldEmitUTF8Identifier: false)) + { + AutoFlush = true + }; + + WriteDirect("===== FASS2 车体模拟器会话开始 ====="); + WriteDirect($"时间: {DateTime.Now:yyyy-MM-dd HH:mm:ss}"); + WriteDirect($"车号: {config.VehicleCode}"); + WriteDirect($"车体监听: {config.VehicleListenAddress}:{config.VehicleListenPort}"); + WriteDirect($"调度目标: {config.SchedulerHost}:{config.SchedulerListenPort}"); + WriteDirect($"上报周期: {config.ReportIntervalMs} ms"); + WriteDirect($"日志文件: {CurrentFilePath}"); + WriteDirect("===================================="); + } + } + + public static void Write(string message) + { + if (!_enabled || string.IsNullOrEmpty(message)) + { + return; + } + + lock (SyncRoot) + { + if (_writer == null) + { + EnsureDefaultWriter(); + } + + if (_writer == null) + { + return; + } + + _writer.WriteLine(message); + } + } + + public static void Shutdown() + { + lock (SyncRoot) + { + if (_writer != null) + { + WriteDirect($"===== 会话结束 {DateTime.Now:yyyy-MM-dd HH:mm:ss} ====="); + } + + CloseWriter(); + } + } + + public static string GetLogDirectory() + { + return _logDirectory ?? ResolveLogDirectory("logs"); + } + + private static void EnsureDefaultWriter() + { + if (!_enabled) + { + return; + } + + Directory.CreateDirectory(_logDirectory); + var fileName = $"app_{DateTime.Now:yyyyMMdd}.log"; + CurrentFilePath = Path.Combine(_logDirectory, fileName); + _writer = new StreamWriter(CurrentFilePath, true, new UTF8Encoding(encoderShouldEmitUTF8Identifier: false)) + { + AutoFlush = true + }; + WriteDirect($"[{DateTime.Now:HH:mm:ss}] 文件日志已启用: {CurrentFilePath}"); + } + + private static void WriteDirect(string message) + { + _writer?.WriteLine(message); + } + + private static void CloseWriter() + { + try + { + _writer?.Flush(); + _writer?.Dispose(); + } + catch + { + } + + _writer = null; + } + + private static string ResolveLogDirectory(string configuredPath) + { + if (string.IsNullOrWhiteSpace(configuredPath)) + { + return Path.Combine(AppContext.BaseDirectory, "logs"); + } + + return Path.IsPathRooted(configuredPath) + ? configuredPath + : Path.Combine(AppContext.BaseDirectory, configuredPath); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimLog.cs b/StandardScene.Fass2Simulator/Fass2SimLog.cs new file mode 100644 index 0000000..4e50e17 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimLog.cs @@ -0,0 +1,33 @@ +using System; + +namespace StandardScene.Fass2Simulator +{ + public static class Fass2SimLog + { + public static event Action MessageWritten; + + public static void WriteLine(string message) + { + if (string.IsNullOrEmpty(message)) + { + return; + } + + Fass2SimFileLogger.Write(message); + MessageWritten?.Invoke(message); + } + + /// + /// 高频日志只写文件，避免 UI 线程被 BeginInvoke 淹没。 + /// + public static void WriteFileOnly(string message) + { + if (string.IsNullOrEmpty(message)) + { + return; + } + + Fass2SimFileLogger.Write(message); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs b/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs new file mode 100644 index 0000000..caa4d1d --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimMotionEngine.cs @@ -0,0 +1,319 @@ +using System; + +namespace StandardScene.Fass2Simulator +{ + /// + /// 按 0xB1 节点序列模拟行驶：距离推进 → 到站更新 Node/字段 → State 切为停止供调度判定到站。 + /// + 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(); + 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"); + } + + /// + /// 供界面展示路段进度：行驶中返回目标节点与路段长度，到站后 node 才会在协议里跳变。 + /// + 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; + } + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimNodeMessage.cs b/StandardScene.Fass2Simulator/Fass2SimNodeMessage.cs new file mode 100644 index 0000000..5710fd1 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimNodeMessage.cs @@ -0,0 +1,121 @@ +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimNodeMessage + { + 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 static Fass2SimNodeMessage FromBytes(byte[] bytes, int offset) + { + return new Fass2SimNodeMessage + { + Node = ReadUInt16(bytes, offset), + Distance = ReadUInt16(bytes, offset + 2), + StartStop = bytes[offset + 4], + Direction = bytes[offset + 5], + Orientation = bytes[offset + 6], + Byroad = bytes[offset + 7], + Speed = ReadUInt16(bytes, offset + 8), + Obstacle = bytes[offset + 10], + Audio = bytes[offset + 11], + Light = bytes[offset + 12], + Charge = bytes[offset + 13], + Rest = bytes[offset + 14], + Lift = bytes[offset + 15], + Clamp = bytes[offset + 16], + Tray = bytes[offset + 17], + Roll = bytes[offset + 18], + Shutdown = bytes[offset + 19] + }; + } + + public void ApplyTo(Fass2SimVehicle vehicle) + { + if (vehicle.Node != Node) + { + return; + } + + if (StartStop > 0) + { + vehicle.StartStop = StartStop; + } + if (Direction > 0) + { + vehicle.Direction = Direction; + } + if (Orientation > 0) + { + vehicle.Orientation = Orientation; + } + if (Byroad > 0) + { + vehicle.Byroad = Byroad; + } + if (Speed > 0) + { + vehicle.Speed = Speed; + } + if (Obstacle > 0) + { + vehicle.Obstacle = Obstacle; + } + if (Audio > 0) + { + vehicle.Audio = Audio; + } + if (Light > 0) + { + vehicle.Light = Light; + } + if (Charge > 0) + { + vehicle.Charge = Charge; + } + if (Rest > 0) + { + vehicle.Rest = Rest; + } + if (Lift > 0) + { + vehicle.Lift = Lift; + } + if (Clamp > 0) + { + vehicle.Clamp = Clamp; + } + if (Tray > 0) + { + vehicle.Tray = Tray; + } + if (Roll > 0) + { + vehicle.Roll = Roll; + } + if (Shutdown > 0) + { + vehicle.Shutdown = Shutdown; + } + } + + private static ushort ReadUInt16(byte[] bytes, int offset) + { + return System.BitConverter.ToUInt16(bytes, offset); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimNodeProfileStore.cs b/StandardScene.Fass2Simulator/Fass2SimNodeProfileStore.cs new file mode 100644 index 0000000..7c2ef0b --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimNodeProfileStore.cs @@ -0,0 +1,44 @@ +using System; +using System.Collections.Generic; + +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimNodeProfile + { + public int ActionDelayMs { get; set; } + } + + public sealed class Fass2SimNodeProfileStore + { + private readonly Dictionary _profiles = new Dictionary(); + + public void Load(IDictionary nodes) + { + _profiles.Clear(); + if (nodes == null) + { + return; + } + + foreach (var pair in nodes) + { + if (!ushort.TryParse(pair.Key, out var nodeId)) + { + continue; + } + + _profiles[nodeId] = pair.Value ?? new Fass2SimNodeProfile(); + } + } + + public int ResolveActionDelayMs(ushort nodeId, int fallbackMs) + { + if (_profiles.TryGetValue(nodeId, out var profile) && profile.ActionDelayMs > 0) + { + return profile.ActionDelayMs; + } + + return fallbackMs; + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimProtocol.cs b/StandardScene.Fass2Simulator/Fass2SimProtocol.cs new file mode 100644 index 0000000..b6c1d3e --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimProtocol.cs @@ -0,0 +1,228 @@ +using System; +using System.Linq; +using System.Text; + +namespace StandardScene.Fass2Simulator +{ + /// + /// FASS 2.0 PCB/UDP 报文编解码，字段布局对齐 + /// StandardScene.Magnetic.Protocol.Fass2Protocol.ParseState 与 backend PCB 仿真器。 + /// + 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(); + 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); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimRuntime.cs b/StandardScene.Fass2Simulator/Fass2SimRuntime.cs new file mode 100644 index 0000000..6fe4170 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimRuntime.cs @@ -0,0 +1,50 @@ +using System; + +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimRuntime : IDisposable + { + private readonly Fass2SimUdpHost _host; + + public Fass2SimRuntime(Fass2SimConfig config, Fass2SimNodeProfileStore profiles) + { + Config = config; + Vehicle = new Fass2SimVehicle(config); + Actions = new Fass2SimActionEngine(Vehicle, config, profiles); + Motion = new Fass2SimMotionEngine(Vehicle, config, Actions); + Actions.StationActionsCompleted += Motion.OnStationActionsCompleted; + var commandHandler = new Fass2SimCommandHandler(Vehicle, Motion, Actions, config.LogRawFrames); + _host = new Fass2SimUdpHost(config, Vehicle, commandHandler, Motion, Actions); + _host.StatusReported += (_, __) => StatusChanged?.Invoke(); + } + + public Fass2SimConfig Config { get; } + + public Fass2SimVehicle Vehicle { get; } + + public Fass2SimMotionEngine Motion { get; } + + public Fass2SimActionEngine Actions { get; } + + public bool IsRunning => _host.IsRunning; + + public event Action StatusChanged; + + public void Start() + { + _host.Start(); + StatusChanged?.Invoke(); + } + + public void Stop() + { + _host.Stop(); + StatusChanged?.Invoke(); + } + + public void Dispose() + { + _host.Dispose(); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs b/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs new file mode 100644 index 0000000..dacef71 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimSelfTests.cs @@ -0,0 +1,123 @@ +using System; + +namespace StandardScene.Fass2Simulator +{ + public static class Fass2SimSelfTests + { + public static int Run(string[] args) + { + if (HasArg(args, "--motion-test")) + { + return RunMotionSelfTest(); + } + + if (HasArg(args, "--action-test")) + { + return RunActionSelfTest(); + } + + return 1; + } + + private static int RunMotionSelfTest() + { + var config = new Fass2SimConfig + { + InitialNode = 1, + DefaultSegmentDistance = 500, + DefaultSpeed = 500, + AutoContinueOnPass = true + }; + var (vehicle, motion, _) = Fass2SimBootstrap.CreateTestStack(config); + var nodes = new[] + { + new Fass2SimNodeMessage { Node = 1, StartStop = 1 }, + new Fass2SimNodeMessage { Node = 2, StartStop = 1, Distance = 500 }, + new Fass2SimNodeMessage { Node = 3, StartStop = 2, Distance = 500 } + }; + + vehicle.State = 1; + motion.OnNodesCommand(1001, nodes); + + for (var i = 0; i < 30; i++) + { + motion.Tick(200); + var snap = vehicle.Snapshot(); + if (snap.Node == 3 && snap.State == 2) + { + Console.WriteLine($"motion self-test ok: node={snap.Node}, task={snap.Task}"); + return 0; + } + } + + var last = vehicle.Snapshot(); + Console.WriteLine($"motion self-test failed: node={last.Node}, state={last.State}, task={last.Task}"); + return 1; + } + + private static int RunActionSelfTest() + { + var config = new Fass2SimConfig + { + InitialNode = 3, + ActionDelayMs = 500, + AutoCompleteStationActions = false + }; + var (vehicle, _, actions) = Fass2SimBootstrap.CreateTestStack(config); + vehicle.State = 2; + vehicle.Node = 3; + vehicle.Lift = 0; + + var station = new Fass2SimNodeMessage + { + Node = 3, + StartStop = Fass2SimActionResolver.StartStopStop, + Lift = 1 + }; + actions.OnStationArrival(station); + + var pendingBefore = Fass2SimActionResolver.DescribePending(station, vehicle.Snapshot()); + if (string.IsNullOrEmpty(pendingBefore) || vehicle.Lift != 0) + { + Console.WriteLine($"action self-test failed at setup: pending={pendingBefore}, lift={vehicle.Lift}"); + return 1; + } + + actions.OnActionCommand(2001, new Fass2SimNodeMessage { Node = 3, Lift = 1 }); + + for (var i = 0; i < 10; i++) + { + actions.Tick(100); + } + + var snap = vehicle.Snapshot(); + if (snap.Lift == 1 && actions.IsStationComplete()) + { + Console.WriteLine($"action self-test ok: node={snap.Node}, lift={snap.Lift}"); + return 0; + } + + Console.WriteLine( + $"action self-test failed: lift={snap.Lift}, complete={actions.IsStationComplete()}, pending={Fass2SimActionResolver.DescribePending(station, snap)}"); + return 1; + } + + private static bool HasArg(string[] args, string name) + { + if (args == null) + { + return false; + } + + foreach (var arg in args) + { + if (string.Equals(arg, name, StringComparison.OrdinalIgnoreCase)) + { + return true; + } + } + + return false; + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs b/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs new file mode 100644 index 0000000..124cd68 --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimUdpHost.cs @@ -0,0 +1,186 @@ +using System; +using System.Net; +using System.Net.Sockets; +using System.Threading; + +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimUdpHost : IDisposable + { + private readonly Fass2SimConfig _config; + private readonly Fass2SimVehicle _vehicle; + private readonly Fass2SimCommandHandler _commandHandler; + private readonly Fass2SimMotionEngine _motionEngine; + private readonly Fass2SimActionEngine _actionEngine; + private readonly IPEndPoint _schedulerEndpoint; + + private UdpClient _listener; + private UdpClient _sender; + private Thread _receiveThread; + private Timer _reportTimer; + private volatile bool _running; + + public Fass2SimUdpHost( + Fass2SimConfig config, + Fass2SimVehicle vehicle, + Fass2SimCommandHandler commandHandler, + Fass2SimMotionEngine motionEngine, + Fass2SimActionEngine actionEngine) + { + _config = config; + _vehicle = vehicle; + _commandHandler = commandHandler; + _motionEngine = motionEngine; + _actionEngine = actionEngine; + _schedulerEndpoint = new IPEndPoint(IPAddress.Parse(config.SchedulerHost), config.SchedulerListenPort); + } + + public bool IsRunning => _running; + + public event EventHandler StatusReported; + + public void Start() + { + if (_running) + { + return; + } + + var listenAddress = IPAddress.Parse(_config.VehicleListenAddress); + _listener = new UdpClient(new IPEndPoint(listenAddress, _config.VehicleListenPort)); + _sender = new UdpClient(); + _running = true; + + _receiveThread = new Thread(ReceiveLoop) + { + IsBackground = true, + Name = $"Fass2SimRx:{_config.VehicleListenPort}" + }; + _receiveThread.Start(); + + var interval = Math.Max(50, _config.ReportIntervalMs); + _reportTimer = new Timer(SendStateReport, null, 0, interval); + + Fass2SimLog.WriteLine($"[{Now()}] 模拟车已启动"); + Fass2SimLog.WriteLine($" 车体监听 : {_config.VehicleListenAddress}:{_config.VehicleListenPort}"); + Fass2SimLog.WriteLine($" 上报目标 : {_config.SchedulerHost}:{_config.SchedulerListenPort}"); + Fass2SimLog.WriteLine($" 车号 : {_config.VehicleCode}"); + Fass2SimLog.WriteLine($" 上报周期 : {interval} ms"); + Fass2SimLog.WriteLine($" 路段模拟 : dist={_config.DefaultSegmentDistance}mm 或 node.Distance, speed={_config.DefaultSpeed}mm/s"); + Fass2SimLog.WriteLine($" 动作延时 : {_config.ActionDelayMs}ms（0xA1 到位），AutoComplete={_config.AutoCompleteStationActions}"); + Fass2SimLog.WriteLine($" 初始节点 : {_config.InitialNode}, 状态={Fass2SimProtocol.StateText(_config.InitialState)}"); + Fass2SimLog.WriteLine("等待 MagFass2Car 联调（address=127.0.0.1, Port=5000, ListenPort=20103, VehicleCode=1）"); + } + + public void Stop() + { + _running = false; + _reportTimer?.Dispose(); + _reportTimer = null; + + try + { + _listener?.Close(); + } + catch + { + } + + try + { + _receiveThread?.Join(1000); + } + catch + { + } + + _receiveThread = null; + _listener = null; + + try + { + _sender?.Close(); + } + catch + { + } + + _sender = null; + Fass2SimLog.WriteLine($"[{Now()}] 模拟车已停止"); + } + + public void Dispose() + { + Stop(); + } + + private void ReceiveLoop() + { + while (_running) + { + try + { + var remote = new IPEndPoint(IPAddress.Any, 0); + var packet = _listener.Receive(ref remote); + _commandHandler.Handle(packet, remote.ToString()); + } + catch (SocketException) when (!_running) + { + break; + } + catch (ObjectDisposedException) when (!_running) + { + break; + } + catch (Exception ex) + { + Fass2SimLog.WriteLine($"[{Now()}] 接收异常: {ex.Message}"); + } + } + } + + private void SendStateReport(object _) + { + if (!_running) + { + return; + } + + try + { + var interval = Math.Max(50, _config.ReportIntervalMs); + _actionEngine.Tick(interval); + _motionEngine.Tick(interval); + + var snapshot = _vehicle.Snapshot(); + var frame = Fass2SimProtocol.BuildState(snapshot); + _sender.Send(frame, frame.Length, _schedulerEndpoint); + _vehicle.RecordStateReportSent(); + StatusReported?.Invoke(this, EventArgs.Empty); + + if (snapshot.StateReportsSent == 0 || snapshot.StateReportsSent % 25 == 0) + { + var pending = _actionEngine.ExpectedStation == null + ? "-" + : Fass2SimActionResolver.DescribePending(_actionEngine.ExpectedStation, snapshot); + if (string.IsNullOrEmpty(pending)) + { + pending = "-"; + } + + Fass2SimLog.WriteLine( + $"[{Now()}] TX 状态 #{snapshot.StateReportsSent + 1}: node={snapshot.Node}, dist={snapshot.Distance}, state={Fass2SimProtocol.StateText(snapshot.State)}, lift={snapshot.Lift}, pending={pending}, task={snapshot.Task}"); + } + } + catch (Exception ex) + { + Fass2SimLog.WriteLine($"[{Now()}] 上报异常: {ex.Message}"); + } + } + + private static string Now() + { + return DateTime.Now.ToString("HH:mm:ss.fff"); + } + } +} diff --git a/StandardScene.Fass2Simulator/Fass2SimVehicle.cs b/StandardScene.Fass2Simulator/Fass2SimVehicle.cs new file mode 100644 index 0000000..8ff41ea --- /dev/null +++ b/StandardScene.Fass2Simulator/Fass2SimVehicle.cs @@ -0,0 +1,171 @@ +using System; + +namespace StandardScene.Fass2Simulator +{ + public sealed class Fass2SimVehicle + { + private readonly object _syncRoot = new object(); + + public Fass2SimVehicle(Fass2SimConfig config) + { + Car = config.VehicleCode; + Length = config.CarLength; + Width = config.CarWidth; + BatteryCharge = config.BatteryCharge; + BatteryHealth = 100; + BatteryVoltage = 480; + State = config.InitialState; + Node = config.InitialNode; + Speed = 500; + } + + public ushort Car { get; } + 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 byte LastCommand { get; set; } + + 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 long StateReportsSent { get; private set; } + public long CommandsReceived { get; private set; } + public long AckReceived { get; private set; } + public DateTime LastAckUtc { get; private set; } = DateTime.MinValue; + + public void RecordStateReportSent() + { + lock (_syncRoot) + { + StateReportsSent++; + } + } + + public void RecordCommandReceived() + { + lock (_syncRoot) + { + CommandsReceived++; + } + } + + public void RecordAckReceived() + { + lock (_syncRoot) + { + AckReceived++; + LastAckUtc = DateTime.UtcNow; + } + } + + public Fass2SimVehicleSnapshot Snapshot() + { + lock (_syncRoot) + { + return CopySnapshotUnsafe(); + } + } + + /// + /// 供已持有其它引擎锁的代码路径读取快照，避免与 UI 线程 Snapshot() 争用 _syncRoot 导致死锁。 + /// + internal Fass2SimVehicleSnapshot CopySnapshotUnsafe() + { + return new Fass2SimVehicleSnapshot + { + Car = Car, + Length = Length, + Width = Width, + BatteryCharge = BatteryCharge, + BatteryHealth = BatteryHealth, + BatteryCurrent = BatteryCurrent, + BatteryVoltage = BatteryVoltage, + HeadingAngle = HeadingAngle, + State = State, + Alarm = Alarm, + Task = Task, + LastCommand = LastCommand, + Node = Node, + Distance = Distance, + StartStop = StartStop, + Direction = Direction, + Orientation = Orientation, + Byroad = Byroad, + Speed = Speed, + Obstacle = Obstacle, + Audio = Audio, + Light = Light, + Charge = Charge, + Rest = Rest, + Lift = Lift, + Clamp = Clamp, + Tray = Tray, + Roll = Roll, + Shutdown = Shutdown, + StateReportsSent = StateReportsSent, + CommandsReceived = CommandsReceived, + AckReceived = AckReceived, + LastAckUtc = LastAckUtc + }; + } + } + + public sealed class Fass2SimVehicleSnapshot + { + 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 byte LastCommand { get; set; } + 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 long StateReportsSent { get; set; } + public long CommandsReceived { get; set; } + public long AckReceived { get; set; } + public DateTime LastAckUtc { get; set; } + } +} diff --git a/StandardScene.Fass2Simulator/MainWindow.xaml b/StandardScene.Fass2Simulator/MainWindow.xaml new file mode 100644 index 0000000..ec20c04 --- /dev/null +++ b/StandardScene.Fass2Simulator/MainWindow.xaml @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + +