diff --git a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasDriver.cs b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasDriver.cs index 76a37e81..f619dadb 100644 --- a/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasDriver.cs +++ b/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasDriver.cs @@ -655,6 +655,12 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery, var sys = await client.GetSysInfoAsync(ct).ConfigureAwait(false); var axes = await client.GetAxisNamesAsync(ct).ConfigureAwait(false); + // Per-axis decimal-place figures (cnc_getfigure), fetched once. Auto figures win + // over the manual PositionDecimalPlaces config at the publish seam; an empty list + // (the managed wire backend today) makes every axis fall back to the config knob. + // Defensive: a figure-read failure must NOT fault device init — default to empty. + state.PositionFigures = await SafeProbe(() => client.GetPositionFiguresAsync(ct), []); + // Optional-API probes — each returns empty / throws when unsupported. var spindles = await SafeProbe(() => client.GetSpindleNamesAsync(ct), []); var spindleMaxRpms = await SafeProbe(() => client.GetSpindleMaxRpmsAsync(ct), []); @@ -718,7 +724,7 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery, var axisIndex = i + 1; // FOCAS uses 1-based axis indexing var axis = cache.Axes[i]; var snap = await client.ReadDynamicAsync(axisIndex, ct).ConfigureAwait(false); - PublishAxisSnapshot(state, axis, snap); + PublishAxisSnapshot(state, axis, snap, i); // i = 0-based axis index → PositionFigures lookup if (i == 0) { firstAxisSnap = snap; PublishRateSnapshot(state, snap); } } @@ -816,24 +822,41 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery, /// → path, which hits /// and returns these cached values. /// - private static void PublishAxisSnapshot(DeviceState state, FocasAxisName axis, FocasDynamicSnapshot snap) + private static void PublishAxisSnapshot(DeviceState state, FocasAxisName axis, FocasDynamicSnapshot snap, int axisIndex) { var host = state.Options.HostAddress; - // cnc_rddynamic2 returns positions as scaled integers; divide by - // 10^PositionDecimalPlaces so they surface in engineering units on the Float64 - // axis nodes. PositionDecimalPlaces is clamped non-negative at config parse, and a - // value of 0 yields factor 1.0 — i.e. the integer widened to double, byte-identical - // to legacy behaviour (12345 / 1.0 == 12345.0). FeedRate / SpindleSpeed (rate - // snapshot) and ServoLoad are NOT position-scaled and are published elsewhere. - var factor = state.Options.PositionDecimalPlaces > 0 - ? Math.Pow(10, state.Options.PositionDecimalPlaces) - : 1.0; + // cnc_rddynamic2 returns positions as scaled integers; divide by 10^figures so they + // surface in engineering units on the Float64 axis nodes. The figure is per-axis: + // an auto cnc_getfigure figure WINS, and the configured PositionDecimalPlaces is the + // fallback when the CNC didn't report one for that axis (see AxisFactor). A figure of + // 0 yields factor 1.0 — i.e. the integer widened to double, byte-identical to legacy + // behaviour (12345 / 1.0 == 12345.0). CAVEAT: the managed WireFocasClient returns no + // figures today, so the REAL backend always uses the manual fallback; live auto-fetch + // lands when a FocasWireClient cnc_getfigure wire command is added. FeedRate / + // SpindleSpeed (rate snapshot) and ServoLoad are NOT position-scaled (published elsewhere). + var factor = AxisFactor(state, axisIndex); state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axis.Display}/AbsolutePosition")] = snap.AbsolutePosition / factor; state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axis.Display}/MachinePosition")] = snap.MachinePosition / factor; state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axis.Display}/RelativePosition")] = snap.RelativePosition / factor; state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axis.Display}/DistanceToGo")] = snap.DistanceToGo / factor; } + /// + /// Resolve the position-scale factor (10^figure) for a single axis. Auto + /// (cnc_getfigure) wins per-axis; manual PositionDecimalPlaces is the + /// fallback when the CNC didn't report a figure for that axis. Both clamp non-negative; + /// 0 ⇒ factor 1.0 (legacy byte-identical). The managed wire backend returns no figures + /// today, so the real backend always takes the manual-fallback branch. + /// + private static double AxisFactor(DeviceState state, int axisIndex) + { + var figures = state.PositionFigures; + var dp = axisIndex >= 0 && axisIndex < figures.Count && figures[axisIndex] >= 0 + ? figures[axisIndex] + : Math.Max(0, state.Options.PositionDecimalPlaces); + return dp > 0 ? Math.Pow(10, dp) : 1.0; + } + private static void PublishRateSnapshot(DeviceState state, FocasDynamicSnapshot snap) { var host = state.Options.HostAddress; @@ -1176,6 +1199,13 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery, public Dictionary LastServoLoads { get; } = new(StringComparer.OrdinalIgnoreCase); /// Gets the last spindle load percentages by spindle index. public Dictionary LastSpindleLoads { get; } = []; + /// + /// Gets or sets the per-axis position decimal-place figures fetched once at init via + /// cnc_getfigure (parallel to the axis-name list; index = axis). An auto figure + /// for an axis WINS over the configured PositionDecimalPlaces; an empty list (the + /// managed wire backend's behaviour today) makes every axis fall back to that config knob. + /// + public IReadOnlyList PositionFigures { get; set; } = []; /// Disposes the FOCAS client instance. public void DisposeClient() diff --git a/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests/FocasPositionAutoScaleTests.cs b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests/FocasPositionAutoScaleTests.cs new file mode 100644 index 00000000..756d8648 --- /dev/null +++ b/tests/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests/FocasPositionAutoScaleTests.cs @@ -0,0 +1,164 @@ +using Shouldly; +using Xunit; +using ZB.MOM.WW.OtOpcUa.Driver.FOCAS; + +namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests; + +/// +/// Phase 4b — per-axis auto-scale from cnc_getfigure figures. The driver fetches +/// per-axis position decimal-place figures at init and applies them at the +/// publish seam. Precedence: an auto figure for an axis WINS; +/// the configured PositionDecimalPlaces is the per-axis fallback when the CNC did +/// not report a figure for that axis. (The managed wire backend returns no figures today, +/// so the real backend always uses the manual fallback — these tests drive the Fake.) +/// +[Trait("Category", "Unit")] +public sealed class FocasPositionAutoScaleTests +{ + private const string Host = "focas://10.0.0.5:8193"; + + // A single-axis CNC (X only) so the snapshot we seed is unambiguous. + private static FakeFocasClient SingleAxisClient( + FocasDynamicSnapshot snap, IReadOnlyList figures) + { + var c = new FakeFocasClient(); + c.AxisNames.Clear(); + c.AxisNames.Add(new("X", "")); + c.DynamicByAxis[1] = snap; // FOCAS axis indexing is 1-based + c.PositionFigures = figures; + return c; + } + + // A two-axis CNC (X, Y) — each axis gets its own seeded snapshot + figure. + private static FakeFocasClient TwoAxisClient( + FocasDynamicSnapshot xSnap, FocasDynamicSnapshot ySnap, IReadOnlyList figures) + { + var c = new FakeFocasClient(); + c.AxisNames.Clear(); + c.AxisNames.Add(new("X", "")); + c.AxisNames.Add(new("Y", "")); + c.DynamicByAxis[1] = xSnap; // axis 0 → 1-based index 1 + c.DynamicByAxis[2] = ySnap; // axis 1 → 1-based index 2 + c.PositionFigures = figures; + return c; + } + + private static FocasDynamicSnapshot AbsSnap(int axisIndex, int absolutePosition) => + new(AxisIndex: axisIndex, AlarmFlags: 0, ProgramNumber: 0, MainProgramNumber: 0, + SequenceNumber: 0, ActualFeedRate: 0, ActualSpindleSpeed: 0, + AbsolutePosition: absolutePosition, MachinePosition: 0, + RelativePosition: 0, DistanceToGo: 0); + + private static FocasDriver NewDriver(int positionDecimalPlaces, FakeFocasClientFactory factory) + { + return new FocasDriver(new FocasDriverOptions + { + Devices = [new FocasDeviceOptions(Host, PositionDecimalPlaces: positionDecimalPlaces)], + Probe = new FocasProbeOptions { Enabled = false }, + FixedTree = new FocasFixedTreeOptions + { + Enabled = true, + PollInterval = TimeSpan.FromMilliseconds(10), + }, + }, "drv-1", factory); + } + + // Drive the fixed-tree poll loop and wait (bounded) for the cached value to appear. + private static async Task ReadWhenPublishedAsync(FocasDriver drv, string reference) + { + var deadline = DateTime.UtcNow + TimeSpan.FromSeconds(5); + while (DateTime.UtcNow < deadline) + { + var snaps = await drv.ReadAsync([reference], CancellationToken.None); + var snap = snaps.Single(); + if (snap.StatusCode == FocasStatusMapper.Good && snap.Value is not null) + return snap.Value; + await Task.Delay(15); + } + return null; + } + + /// + /// An auto figure for an axis WINS over the configured manual value: figure 3 ⇒ ÷10^3 + /// (12345 → 12.345), even though PositionDecimalPlaces: 1 would have given ÷10. + /// + [Fact] + public async Task Auto_figure_wins_over_manual_config() + { + var factory = new FakeFocasClientFactory + { + Customise = () => SingleAxisClient(AbsSnap(1, 12345), figures: [3]), + }; + var drv = NewDriver(positionDecimalPlaces: 1, factory); + await drv.InitializeAsync("{}", CancellationToken.None); + + var abs = await ReadWhenPublishedAsync(drv, $"{Host}/Axes/X/AbsolutePosition"); + abs.ShouldBe(12.345); // ÷10^3 (auto), NOT 1234.5 (manual ÷10) + + await drv.ShutdownAsync(CancellationToken.None); + } + + /// + /// When cnc_getfigure reports nothing (empty list — the real wire backend's + /// behaviour), the driver falls back to the configured PositionDecimalPlaces: + /// manual 2 ⇒ ÷10^2 (12345 → 123.45). + /// + [Fact] + public async Task Falls_back_to_manual_when_getfigure_empty() + { + var factory = new FakeFocasClientFactory + { + Customise = () => SingleAxisClient(AbsSnap(1, 12345), figures: []), + }; + var drv = NewDriver(positionDecimalPlaces: 2, factory); + await drv.InitializeAsync("{}", CancellationToken.None); + + var abs = await ReadWhenPublishedAsync(drv, $"{Host}/Axes/X/AbsolutePosition"); + abs.ShouldBe(123.45); // ÷10^2 (manual fallback) + + await drv.ShutdownAsync(CancellationToken.None); + } + + /// + /// Figures are applied per-axis independently: figures [3, 1] ⇒ axis 0 ÷10^3 and + /// axis 1 ÷10^1. + /// + [Fact] + public async Task Per_axis_figures_scale_independently() + { + var factory = new FakeFocasClientFactory + { + Customise = () => TwoAxisClient( + AbsSnap(1, 12345), AbsSnap(2, 67890), figures: [3, 1]), + }; + var drv = NewDriver(positionDecimalPlaces: 0, factory); + await drv.InitializeAsync("{}", CancellationToken.None); + + var x = await ReadWhenPublishedAsync(drv, $"{Host}/Axes/X/AbsolutePosition"); + var y = await ReadWhenPublishedAsync(drv, $"{Host}/Axes/Y/AbsolutePosition"); + x.ShouldBe(12.345); // ÷10^3 + y.ShouldBe(6789.0); // ÷10^1 + + await drv.ShutdownAsync(CancellationToken.None); + } + + /// + /// The legacy manual-only path is unchanged: no auto figures + PositionDecimalPlaces: 0 + /// ⇒ factor 1.0, so the raw integer is published widened to double (12345 → 12345.0). + /// + [Fact] + public async Task Manual_only_legacy_path_unchanged() + { + var factory = new FakeFocasClientFactory + { + Customise = () => SingleAxisClient(AbsSnap(1, 12345), figures: []), + }; + var drv = NewDriver(positionDecimalPlaces: 0, factory); + await drv.InitializeAsync("{}", CancellationToken.None); + + var abs = await ReadWhenPublishedAsync(drv, $"{Host}/Axes/X/AbsolutePosition"); + abs.ShouldBe(12345.0); // factor 1.0 — raw widened to double + + await drv.ShutdownAsync(CancellationToken.None); + } +}