zhaowei.huang/StandardSence
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: FASS2 状态机与 UDP 枢纽审查项

Browse Source 
等锁超时、全路径签名、无锁窗口不下发、Fault 终态可等待、Pass+机构不再短路、Alarm 不杀单、catch-up 停在未完成站；VehicleCode=0/重复车号拒绝注册，Hub 已运行时不抢监听口；FileLogger 空目录可写。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
黄兆尉
2026-08-26 23:04:58 +08:00
co-authored by Cursor
parent 29a06f004f
commit e4a245e644
13 changed files with 465 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
StandardScene.Magnetic/CarTypes/Mag2Car.cs
+38 -3
View File
@@ -44,6 +44,7 @@ namespace StandardScene.CarTypes
private DateTime _lastPollTime = DateTime.MinValue;
private DateTime _lastUdpReportTime = DateTime.MinValue;
private bool _udpRegistered;
private bool _udpZeroCodeWarned;
private ushort _registeredVehicleCode = ushort.MaxValue;
private long _nextTaskId = 1;
private long _nextActionId = 1;
@@ -71,6 +72,7 @@ namespace StandardScene.CarTypes
[FieldMember] public bool VerifyTrajectoryFieldsOnStation = false;
[FieldMember] public int TaskResendIntervalMs = 500;
[FieldMember] public int ActionRetryIntervalMs = 1000;
[FieldMember] public int TrafficWaitTimeoutMs = 180000;
[FieldMember] public bool EnableTaskStateMachine = true;
[FieldMember] public bool EnableLoopTaskDrive = true;
[FieldMember] public bool StartBeforeMove = true;
@@ -567,6 +569,7 @@ namespace StandardScene.CarTypes
_taskStateMachine.LockCount = LockCount;
_taskStateMachine.ResendIntervalMs = TaskResendIntervalMs;
_taskStateMachine.ActionRetryIntervalMs = ActionRetryIntervalMs;
_taskStateMachine.TrafficWaitTimeoutMs = TrafficWaitTimeoutMs;
_taskStateMachine.StartBeforeMove = StartBeforeMove;
Fass2ActionResolver.VerifyTrajectoryFields = VerifyTrajectoryFieldsOnStation;
_taskStateMachine.Begin(startSiteId, goalSiteId, defaultSpeed);
@@ -1147,6 +1150,21 @@ namespace StandardScene.CarTypes
throw new InvalidOperationException("Mag2Car address is empty for UDP mode");
}
if (VehicleCode == 0)
{
if (!_udpZeroCodeWarned)
{
_udpZeroCodeWarned = true;
Diagnosis.Post(
$"Mag2Car {name}({id}) VehicleCode=0，UDP 未注册。请为每台车配置唯一车号",
"Mag2Car",
true);
DetailLog("UDP register skipped: VehicleCode=0");
}
return;
}
if (!_udpRegistered || _registeredVehicleCode != VehicleCode)
{
if (_udpRegistered && _registeredVehicleCode != VehicleCode)
@@ -1155,11 +1173,28 @@ namespace StandardScene.CarTypes
_udpRegistered = false;
}
Fass2UdpHub.Register(this);
Fass2UdpHub.EnsureStarted(ListenPort);
if (!Fass2UdpHub.Register(this))
{
DetailLog($"UDP register rejected, VehicleCode={VehicleCode}");
return;
}
if (!Fass2UdpHub.IsRunning)
{
Fass2UdpHub.EnsureStarted(ListenPort);
}
else if (ListenPort > 0 && ListenPort != Fass2UdpHub.ListenPort)
{
var msg =
$"Mag2Car {name}({id}) ListenPort={ListenPort} 与已运行 Hub={Fass2UdpHub.ListenPort} 不一致，已忽略本车端口（配置错误）";
Diagnosis.Post(msg, "Mag2Car", true);
DetailLog(
$"UDP listen keep hub={Fass2UdpHub.ListenPort}, car ListenPort={ListenPort} ignored");
}
_udpRegistered = true;
_registeredVehicleCode = VehicleCode;
DetailLog($"UDP registered, VehicleCode={VehicleCode}, listen={ListenPort}, remote={address}:{Port}");
DetailLog($"UDP registered, VehicleCode={VehicleCode}, listen={Fass2UdpHub.ListenPort}, remote={address}:{Port}");
}
}
StandardScene.Magnetic/Protocol/Fass2UdpHub.cs
+52 -10
View File
@@ -23,16 +23,37 @@ namespace StandardScene.Magnetic.Protocol
private static int _listenPort = 20103;
private static readonly Dictionary<ushort, long> _lastUnknownCarLogUtcTicks = new Dictionary<ushort, long>();
public static void Register(IFass2UdpCar car)
internal static bool IsRunning => _running;
internal static int ListenPort => _listenPort;
public static bool Register(IFass2UdpCar car)
{
if (car == null)
{
return;
return false;
}
if (car.VehicleCode == 0)
{
Diagnosis.Post("Fass2UdpHub 拒绝注册 VehicleCode=0，请为每台 Mag2Car 配置唯一车号", "Fass2UdpHub", true);
return false;
}
lock (SyncRoot)
{
if (CarsByCode.TryGetValue(car.VehicleCode, out var existing) &&
!ReferenceEquals(existing, car))
{
Diagnosis.Post(
$"Fass2UdpHub 拒绝覆盖 VehicleCode={car.VehicleCode}（已被另一台车占用）",
"Fass2UdpHub",
true);
return false;
}
CarsByCode[car.VehicleCode] = car;
return true;
}
}
@@ -64,9 +85,31 @@ namespace StandardScene.Magnetic.Protocol
public static void EnsureStarted(int listenPort)
{
if (listenPort > 0)
if (listenPort <= 0)
{
_listenPort = listenPort;
listenPort = _listenPort;
}
var needRestart = false;
lock (SyncRoot)
{
if (_running && listenPort != _listenPort)
{
needRestart = true;
}
else if (_running)
{
return;
}
}
if (needRestart)
{
Diagnosis.Post(
$"Fass2UdpHub 监听口 {_listenPort} → {listenPort}，重启枢纽",
"Fass2UdpHub",
true);
Stop();
}
lock (SyncRoot)
@@ -76,6 +119,7 @@ namespace StandardScene.Magnetic.Protocol
return;
}
_listenPort = listenPort;
_listener = new UdpClient(_listenPort);
_running = true;
_receiveThread = new Thread(ReceiveLoop)
@@ -209,6 +253,7 @@ namespace StandardScene.Magnetic.Protocol
private static void LogUnknownCar(ushort carCode)
{
var nowTicks = DateTime.UtcNow.Ticks;
string registered;
lock (SyncRoot)
{
if (_lastUnknownCarLogUtcTicks.TryGetValue(carCode, out var lastTicks)
@@ -218,12 +263,9 @@ namespace StandardScene.Magnetic.Protocol
}
_lastUnknownCarLogUtcTicks[carCode] = nowTicks;
}
var registered = string.Join(",", CarsByCode.Keys);
if (string.IsNullOrEmpty(registered))
{
registered = "(无)";
registered = CarsByCode.Count == 0
? "(无)"
: string.Join(",", CarsByCode.Keys);
}
Diagnosis.Post(
StandardScene.Magnetic/Tasking/Fass2ActionResolver.cs
-5
View File
@@ -106,11 +106,6 @@ namespace StandardScene.Magnetic.Tasking
return true;
}
if (expected.StartStop == Fass2TaskBuilder.StartStopPass)
{
return true;
}
if (expected.StartStop == Fass2TaskBuilder.StartStopControlStop)
{
return false;
StandardScene.Magnetic/Tasking/Fass2TaskStateMachine.cs
+52 -13
View File
@@ -80,6 +80,9 @@ namespace StandardScene.Magnetic.Tasking
public int ActionRetryIntervalMs { get; set; } = 1000;
/// <summary>交管等锁 / 管控等放行上限。0 表示不限制。</summary>
public int TrafficWaitTimeoutMs { get; set; } = 180_000;
public bool StartBeforeMove { get; set; } = true;
public ushort HeadingAngle { get; set; }
@@ -90,7 +93,8 @@ namespace StandardScene.Magnetic.Tasking
{
lock (_syncRoot)
{
return Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled;
return Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete
or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault;
}
}
}
@@ -327,11 +331,6 @@ namespace StandardScene.Magnetic.Tasking
var result = new Fass2TaskTickResult { Phase = Context.Phase };
if (report?.Alarm != 0)
{
return Fault($"alarm=0x{report.Alarm:X}", result);
}
if (report?.State == 3)
{
return Fault("vehicle emergency stop", result);
@@ -432,14 +431,25 @@ namespace StandardScene.Magnetic.Tasking
public async Task WaitForCompletionAsync(int timeoutMs, CancellationToken cancellationToken = default)
{
TaskCompletionSource<int> waiter;
string failReason;
lock (_syncRoot)
{
if (Context.Phase is Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
if (Context.Phase == Fass2TaskPhase.Complete)
{
return;
}
waiter = _completionSource ?? ResetCompletionSource();
waiter = _completionSource;
failReason = Context.FaultReason;
if (Context.Phase is not Fass2TaskPhase.Cancelled and not Fass2TaskPhase.Fault)
{
waiter = waiter ?? ResetCompletionSource();
}
}
if (waiter == null)
{
throw new InvalidOperationException(failReason ?? "task ended without completion source");
}
await WaitWithTrafficPauseAsync(waiter, timeoutMs, cancellationToken);
@@ -459,6 +469,7 @@ namespace StandardScene.Magnetic.Tasking
var remainingMs = timeoutMs <= 0 ? -1 : timeoutMs;
const int sliceMs = 500;
var lastPauseLog = DateTime.MinValue;
var pauseElapsedMs = 0;
while (true)
{
@@ -500,6 +511,17 @@ namespace StandardScene.Magnetic.Tasking
if (waitingPaused)
{
if (TrafficWaitTimeoutMs > 0)
{
pauseElapsedMs += delayMs;
if (pauseElapsedMs >= TrafficWaitTimeoutMs)
{
Cancel($"{pauseKind} timeout after {TrafficWaitTimeoutMs}ms");
throw new TimeoutException(
$"FASS2 {pauseKind} timeout after {TrafficWaitTimeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
}
}
if ((DateTime.Now - lastPauseLog).TotalSeconds >= 5)
{
lastPauseLog = DateTime.Now;
@@ -509,6 +531,8 @@ namespace StandardScene.Magnetic.Tasking
continue;
}
pauseElapsedMs = 0;
if (remainingMs < 0)
{
continue;
@@ -552,7 +576,13 @@ namespace StandardScene.Magnetic.Tasking
}
catch (Exception ex)
{
return Fault($"traffic prepare failed: {ex.Message}", result);
Context.Phase = Fass2TaskPhase.Moving;
Context.WaitingForTraffic = true;
result.Phase = Context.Phase;
result.Message = $"traffic prepare wait: {ex.Message}";
Log(result.Message);
Persist();
return result;
}
if (StartBeforeMove && Context.CurrentIndex == 0)
@@ -884,7 +914,7 @@ namespace StandardScene.Magnetic.Tasking
}
var newSignature = Fass2SiteFieldReader.BuildFieldsSignature(
Context.Plan.SiteIds, Context.CurrentIndex, SimpleLib.GetSite);
Context.Plan.SiteIds, 0, SimpleLib.GetSite);
if (string.Equals(newSignature, Context.FieldsSignature, StringComparison.Ordinal))
{
return false;
@@ -1005,7 +1035,12 @@ namespace StandardScene.Magnetic.Tasking
lockedCount = Fass2ControlAreaGate.LimitWindowCount(
nodes, start, lockedCount, Context.ControlReleasedIndex);
lockedCount = Math.Max(1, Math.Min(lockedCount, wantWindow));
lockedCount = Math.Max(0, Math.Min(lockedCount, wantWindow));
if (lockedCount <= 0)
{
return Array.Empty<Fass2NodeMessage>();
}
var window = new Fass2NodeMessage[lockedCount];
for (var i = 0; i < lockedCount; i++)
{
@@ -1039,8 +1074,12 @@ namespace StandardScene.Magnetic.Tasking
var catchTo = reportIndex;
for (var i = Context.CurrentIndex; i < reportIndex; i++)
{
if (Fass2ControlAreaGate.IsControlStop(Context.Plan.Nodes[i].StartStop) &&
Context.ControlReleasedIndex != i)
var node = Context.Plan.Nodes[i];
var unreleasedControl = Fass2ControlAreaGate.IsControlStop(node.StartStop) &&
Context.ControlReleasedIndex != i;
var actionStation = Fass2ActionResolver.RequiresActionWait(node) &&
node.StartStop != Fass2TaskBuilder.StartStopControlStart;
if (unreleasedControl || actionStation)
{
catchTo = i;
break;
StandardScene.Magnetic/Tasking/Fass2TrafficLocker.cs
-6
View File
@@ -221,12 +221,6 @@ namespace StandardScene.Magnetic.Tasking
lockedCount++;
}
if (lockedCount == 0)
{
lockedCount = 1;
_log($"window warn: no holding overlap index={fromIndex}, site={siteIds[fromIndex]}, holding=[{FormatInts(_car.status.holdingLocks)}]");
}
_log($"window locked={lockedCount}/{windowSize}, fromIndex={fromIndex}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}]");
return lockedCount;
}
StandardScene.Magnetic/docs/Mag2Car使用指南.md
+7 -6
View File
@@ -65,9 +65,9 @@
| 参数 | 示例值 | 含义 |
|------|--------|------|
| `address` | `192.168.1.100` | 车体 IP |
| `Port` | `5000` | 下发命令的目标端口 |
| `ListenPort` | `20103` | 调度监听车体上报的端口 |
| `VehicleCode` | `1` | 车体编号，须与上报帧一致 |
| `Port` | `5001`（模拟器）/ `5000`（真机常见） | 下发命令的目标端口，须等于车体监听口 |
| `ListenPort` | `20103` | 调度监听车体上报的端口；多车必须相同，不一致会告警并忽略 |
| `VehicleCode` | `1` | 车体编号，须与上报帧一致；`0` 会拒绝 UDP 注册并告警 |
| `CommMode` | `Udp` | PCB/UDP 模式（推荐） |
### 步骤 3：确认在线
@@ -194,9 +194,9 @@ agv.Mag2Go(2, 5, 0.2);
|------|------|------|
| `CommMode` | `Udp` | `Udp`=PCB 被动收状态；`Tcp`=PLC 主动查询 |
| `address` | — | 车体 IP |
| `Port` | `5000` | 命令下发端口 |
| `ListenPort` | `20103` | 调度 UDP 监听端口 |
| `VehicleCode` | `0` | 必须与车体上报的 `Car` 字段一致 |
| `Port` | `5000` | 命令下发端口；联调模拟器时填 `5001`/`5002`（见 `appsettings*.json`） |
| `ListenPort` | `20103` | 调度 UDP 监听端口；Hub 已启动后本车改端口不会重启监听 |
| `VehicleCode` | `0` | 必须与车体上报的 `Car` 字段一致；保持 `0` 会告警且收不到上报 |
| `UdpOfflineTimeoutMs` | `15000` | 超过此时间未收到上报视为离线 |
### 5.2 任务与状态机
@@ -208,6 +208,7 @@ agv.Mag2Go(2, 5, 0.2);
| `LockCount` | `4` | 每次 `0xB1` 滑动窗口站数（≤10） |
| `TaskResendIntervalMs` | `500` | 周期重发 `0xB1` 间隔 |
| `ActionRetryIntervalMs` | `1000` | 动作未完成时 `0xA1` 重试间隔 |
| `TrafficWaitTimeoutMs` | `180000` | 交管等锁 / 管控等待上限；超时取消任务 |
| `MoveTimeoutSeconds` | `120` | 单次移动超时（秒） |
| `StartBeforeMove` | `true` | 移动前发 `0x01` 启动 |