From f320f323aee54c3b8d9825f8f707bff33f3f6672 Mon Sep 17 00:00:00 2001 From: Joseph Doherty Date: Thu, 18 Jun 2026 12:23:14 -0400 Subject: [PATCH] feat(focas): add cnc_getfigure wire command + focas-mock handler --- .../Wire/FocasWireClient.cs | 35 +++++++++++++++++++ .../Wire/WireFocasClient.cs | 26 ++++++++++---- .../focas-mock/src/focas_mock/defaults.py | 4 +++ .../focas-mock/src/focas_mock/server.py | 10 ++++++ 4 files changed, 68 insertions(+), 7 deletions(-) diff --git a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/FocasWireClient.cs b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/FocasWireClient.cs index 390a9bdc..66a40701 100644 --- a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/FocasWireClient.cs +++ b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/FocasWireClient.cs @@ -385,6 +385,41 @@ public sealed class FocasWireClient : IAsyncDisposable, IDisposable return new FocasResult>(rc, result); } + /// + /// Read the per-axis position decimal-place figures via cnc_getfigure + /// (command 0x00d3). The response payload is a self-delimiting sequence of + /// big-endian short decimal-place counts — one per configured axis — which the + /// driver applies as a 10^figure scale on raw position reads. + /// + /// + /// The 0x00d3 command id is co-designed with the in-tree focas_mock sim and + /// validated against it; like the rest of this managed wire backend the binary shape is + /// sim-consistent and has not been validated against a real Fanuc CNC (bench-CNC-gated). + /// + /// Cancellation token for the read operation. + /// Optional per-call timeout override. + /// Optional path ID override; defaults to . + public async Task>> ReadPositionFiguresAsync( + CancellationToken cancellationToken = default, + TimeSpan? timeout = null, + ushort? pathId = null) + { + using var callTimeout = CreateCallTimeout(cancellationToken, timeout); + var requestPathId = EffectivePathId(pathId); + var block = await SendSingleRequestAsync( + callTimeout.Token, + new RequestBlock(0x00d3, PathId: requestPathId)).ConfigureAwait(false); + + if (block.Rc != 0) return new FocasResult>(block.Rc, null); + + var payload = block.Payload; + var figures = new List(payload.Length / 2); + for (var offset = 0; offset + 2 <= payload.Length; offset += 2) + figures.Add(ReadInt16(payload, offset)); + + return new FocasResult>(block.Rc, figures); + } + /// Read per-spindle load percentages via cnc_rdspload (command 0x0040 with arg1=0). /// Spindle selector; -1 selects all spindles. /// Cancellation token for the read operation. diff --git a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/WireFocasClient.cs b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/WireFocasClient.cs index 003a146e..666d8450 100644 --- a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/WireFocasClient.cs +++ b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/WireFocasClient.cs @@ -287,14 +287,26 @@ public sealed class WireFocasClient : IFocasClient /// Gets the per-axis position decimal-place figures via cnc_getfigure. /// Cancellation token for the operation. /// - /// An empty list — the managed FOCAS/2 Ethernet wire client () - /// does not currently expose the cnc_getfigure command, so this backend reports no - /// per-axis figures. Per the contract an - /// empty list signals the driver to fall back to the configured PositionDecimalPlaces. - /// Never throws — figure reads degrade to the config knob rather than faulting. + /// The per-axis decimal-place figures read from the CNC via + /// , or an empty list when the read + /// fails (non-zero RC — e.g. the series lacks cnc_getfigure) or is not connected. + /// Per the contract an empty list signals + /// the driver to fall back to the configured PositionDecimalPlaces. Never throws — + /// figure reads degrade to the config knob rather than faulting. /// - public Task> GetPositionFiguresAsync(CancellationToken cancellationToken) => - Task.FromResult>(Array.Empty()); + public async Task> GetPositionFiguresAsync(CancellationToken cancellationToken) + { + if (!_wire.IsConnected) return []; + try + { + var result = await _wire.ReadPositionFiguresAsync(cancellationToken).ConfigureAwait(false); + return result.IsOk && result.Value is { } figures ? figures : []; + } + catch (FocasWireException) + { + return []; + } + } private static async Task> ReadSpindleMetricAsync( Func>>> call, diff --git a/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/defaults.py b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/defaults.py index d19b5649..45857ac1 100644 --- a/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/defaults.py +++ b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/defaults.py @@ -78,6 +78,10 @@ def make_default_state(profile: Mapping[str, Any]) -> dict[str, Any]: "acts": 3200, "acts2": [3200] + [0] * (max_spindles - 1), "axis_names": axis_names[:max_axis], + # Per-axis position decimal places served by cnc_getfigure (command 0x00d3). + # Default every configured axis to 0 ⇒ 10^0 = 1.0 scale, leaving existing + # position reads unchanged. mock_patch can override individual axes. + "position_figures": {name: 0 for name in axis_names[:max_axis]}, "spindle_names": spindle_names, "parameters": { "6711": {"type": "long", "value": 1, "decimal": 0, "description": "example parameter"}, diff --git a/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/server.py b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/server.py index babb3d57..c3d8e67d 100644 --- a/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/server.py +++ b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.IntegrationTests/Docker/focas-mock/src/focas_mock/server.py @@ -256,6 +256,8 @@ class FocasMockServer: return self._wire_spindle_metric(request_block) if command == 0x56: return self._wire_servo_meter() + if command == 0xD3: + return self._wire_position_figures() if command == 0x57: mode = self.store.snapshot()["operation_mode"] return self._u16(int(mode.get("mode", 0) if isinstance(mode, dict) else mode)) @@ -381,6 +383,14 @@ class FocasMockServer: payload += self._name_record(name, 4) return bytes(payload) + def _wire_position_figures(self) -> bytes: + state = self.store.snapshot() + figures = state.get("position_figures", {}) + payload = bytearray() + for name in state["axis_names"]: + payload += self._u16(int(figures.get(name, 0))) + return bytes(payload) + def _wire_spindle_metric(self, request_block: bytes) -> bytes: metric = self._block_u32(request_block, 8) spindle = self.store.snapshot()["spindle"]