dohertj2/lmxopcua
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Auto: focas-f1d — Tool number + work coordinate offsets

Browse Source 
Closes #260
This commit is contained in:
Joseph Doherty
2026-04-25 14:37:51 -04:00
parent 49fc23adc6
commit 9ec92a9082
6 changed files with 603 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasCapabilityMatrix.cs
View File

@@ -106,6 +106,27 @@ public static class FocasCapabilityMatrix
_ => int.MaxValue,
};
/// <summary>
/// Whether the FOCAS driver should expose the per-device <c>Tooling/</c>
/// fixed-tree subfolder for a given <paramref name="series"/>. Backed by
/// <c>cnc_rdtnum</c>, which is documented for every modern Fanuc series
/// (0i / 16i / 30i families) — defaulting to <c>true</c>. The capability
/// hook exists so a future controller without <c>cnc_rdtnum</c> can opt
/// out without touching the driver. <see cref="FocasCncSeries.Unknown"/>
/// stays permissive (matches the modal / override fixed-tree precedent in
/// issue #259). Issue #260.
/// </summary>
public static bool SupportsTooling(FocasCncSeries series) => true;
/// <summary>
/// Whether the FOCAS driver should expose the per-device <c>Offsets/</c>
/// fixed-tree subfolder for a given <paramref name="series"/>. Backed by
/// <c>cnc_rdzofs(n=1..6)</c> for the standard G54..G59 surfaces; extended
/// G54.1 P1..P48 surfaces are deferred to a follow-up. Same permissive
/// policy as <see cref="SupportsTooling"/>. Issue #260.
/// </summary>
public static bool SupportsWorkOffsets(FocasCncSeries series) => true;
private static string? ValidateMacro(FocasCncSeries series, int number)
{
var (min, max) = MacroRange(series);

188
src/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FocasDriver.cs
View File

@@ -32,6 +32,10 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
new(StringComparer.OrdinalIgnoreCase);
private readonly Dictionary<string, (string Host, string Field)> _overrideNodesByName =
new(StringComparer.OrdinalIgnoreCase);
private readonly Dictionary<string, string> _toolingNodesByName =
new(StringComparer.OrdinalIgnoreCase);
private readonly Dictionary<string, (string Host, string Slot, string Axis)> _offsetNodesByName =
new(StringComparer.OrdinalIgnoreCase);
private DriverHealth _health = new(DriverState.Unknown, null, null);
/// <summary>
@@ -69,6 +73,24 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
/// </summary>
private static readonly string[] OverrideFieldNames = ["Feed", "Rapid", "Spindle", "Jog"];
/// <summary>
/// Names of the standard work-coordinate offset slots surfaced under
/// <c>Offsets/</c> per device — G54..G59 from <c>cnc_rdzofs(n=1..6)</c>
/// (issue #260). Extended G54.1 P1..P48 surfaces are deferred to a follow-up
/// PR because <c>cnc_rdzofsr</c> uses a different range surface.
/// </summary>
private static readonly string[] WorkOffsetSlotNames =
[
"G54", "G55", "G56", "G57", "G58", "G59",
];
/// <summary>
/// Axis columns surfaced under each <c>Offsets/{slot}/</c> folder. Per the F1-d
/// plan a fixed 3-axis (X/Y/Z) view is used; lathes / mills with extra rotational
/// offsets get those columns exposed as 0.0 until a follow-up extends the surface.
/// </summary>
private static readonly string[] WorkOffsetAxisNames = ["X", "Y", "Z"];
public event EventHandler<DataChangeEventArgs>? OnDataChange;
public event EventHandler<HostStatusChangedEventArgs>? OnHostStatusChanged;
@@ -141,6 +163,28 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
(device.Options.HostAddress, field);
}
}
// Tooling/CurrentTool — single Int16 node per device (issue #260). Tool
// life + active offset index are deferred per the F1-d plan; they need
// ODBTLIFE* unions whose shape varies per series.
if (FocasCapabilityMatrix.SupportsTooling(device.Options.Series))
{
_toolingNodesByName[ToolingReferenceFor(device.Options.HostAddress, "CurrentTool")] =
device.Options.HostAddress;
}
// Offsets/{G54..G59}/{X|Y|Z} — fixed 3-axis view of the standard work-
// coordinate offsets (issue #260). Capability matrix gates by series so
// legacy CNCs that don't support cnc_rdzofs don't produce the subtree.
if (FocasCapabilityMatrix.SupportsWorkOffsets(device.Options.Series))
{
foreach (var slot in WorkOffsetSlotNames)
foreach (var axis in WorkOffsetAxisNames)
{
_offsetNodesByName[OffsetReferenceFor(device.Options.HostAddress, slot, axis)] =
(device.Options.HostAddress, slot, axis);
}
}
}
if (_options.Probe.Enabled)
@@ -184,6 +228,8 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
_productionNodesByName.Clear();
_modalNodesByName.Clear();
_overrideNodesByName.Clear();
_toolingNodesByName.Clear();
_offsetNodesByName.Clear();
_health = new DriverHealth(DriverState.Unknown, _health.LastSuccessfulRead, null);
}
@@ -237,6 +283,22 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
continue;
}
// Fixed-tree Tooling/CurrentTool — served from cached cnc_rdtnum snapshot
// refreshed on the probe tick (issue #260). No wire call here.
if (_toolingNodesByName.TryGetValue(reference, out var toolingHost))
{
results[i] = ReadToolingField(toolingHost, "CurrentTool", now);
continue;
}
// Fixed-tree Offsets/{slot}/{axis} — served from cached cnc_rdzofs(1..6)
// snapshot refreshed on the probe tick (issue #260). No wire call here.
if (_offsetNodesByName.TryGetValue(reference, out var offsetKey))
{
results[i] = ReadOffsetField(offsetKey.Host, offsetKey.Slot, offsetKey.Axis, now);
continue;
}
if (!_tagsByName.TryGetValue(reference, out var def))
{
results[i] = new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now);
@@ -435,6 +497,50 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
WriteIdempotent: false));
}
}
// Fixed-tree Tooling/ subfolder — single Int16 CurrentTool node from
// cnc_rdtnum (issue #260). Tool life + active offset index are deferred
// per the F1-d plan because the FWLIB ODBTLIFE* unions vary per series.
if (FocasCapabilityMatrix.SupportsTooling(device.Series))
{
var toolingFolder = deviceFolder.Folder("Tooling", "Tooling");
var toolingRef = ToolingReferenceFor(device.HostAddress, "CurrentTool");
toolingFolder.Variable("CurrentTool", "CurrentTool", new DriverAttributeInfo(
FullName: toolingRef,
DriverDataType: DriverDataType.Int16,
IsArray: false,
ArrayDim: null,
SecurityClass: SecurityClassification.ViewOnly,
IsHistorized: false,
IsAlarm: false,
WriteIdempotent: false));
}
// Fixed-tree Offsets/ subfolder — G54..G59 each with X/Y/Z Float64 axes
// from cnc_rdzofs(n=1..6) (issue #260). Capability matrix gates the surface
// by series so legacy controllers without cnc_rdzofs support don't expose
// dead nodes. Extended G54.1 P1..P48 surfaces are deferred to a follow-up.
if (FocasCapabilityMatrix.SupportsWorkOffsets(device.Series))
{
var offsetsFolder = deviceFolder.Folder("Offsets", "Offsets");
foreach (var slot in WorkOffsetSlotNames)
{
var slotFolder = offsetsFolder.Folder(slot, slot);
foreach (var axis in WorkOffsetAxisNames)
{
var fullRef = OffsetReferenceFor(device.HostAddress, slot, axis);
slotFolder.Variable(axis, axis, new DriverAttributeInfo(
FullName: fullRef,
DriverDataType: DriverDataType.Float64,
IsArray: false,
ArrayDim: null,
SecurityClass: SecurityClassification.ViewOnly,
IsHistorized: false,
IsAlarm: false,
WriteIdempotent: false));
}
}
}
}
return Task.CompletedTask;
}
@@ -451,6 +557,12 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
private static string OverrideReferenceFor(string hostAddress, string field) =>
$"{hostAddress}::Override/{field}";
private static string ToolingReferenceFor(string hostAddress, string field) =>
$"{hostAddress}::Tooling/{field}";
private static string OffsetReferenceFor(string hostAddress, string slot, string axis) =>
$"{hostAddress}::Offsets/{slot}/{axis}";
private static ushort? OverrideParamFor(FocasOverrideParameters p, string field) => field switch
{
"Feed" => p.FeedParam,
@@ -566,6 +678,28 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
state.LastOverrideUtc = DateTime.UtcNow;
}
}
// Tooling/CurrentTool + Offsets/{G54..G59}/{X|Y|Z} — same best-
// effort policy as the other fixed-tree caches (issue #260). A
// null result leaves the previous good snapshot in place so reads
// keep serving until the next successful refresh.
if (FocasCapabilityMatrix.SupportsTooling(state.Options.Series))
{
var tooling = await client.GetToolingAsync(ct).ConfigureAwait(false);
if (tooling is not null)
{
state.LastTooling = tooling;
state.LastToolingUtc = DateTime.UtcNow;
}
}
if (FocasCapabilityMatrix.SupportsWorkOffsets(state.Options.Series))
{
var offsets = await client.GetWorkOffsetsAsync(ct).ConfigureAwait(false);
if (offsets is not null)
{
state.LastWorkOffsets = offsets;
state.LastWorkOffsetsUtc = DateTime.UtcNow;
}
}
}
}
catch (OperationCanceledException) when (ct.IsCancellationRequested) { break; }
@@ -630,6 +764,43 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
device.LastOverrideUtc, now);
}
private DataValueSnapshot ReadToolingField(string hostAddress, string field, DateTime now)
{
if (!_devices.TryGetValue(hostAddress, out var device))
return new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now);
if (device.LastTooling is not { } snap)
return new DataValueSnapshot(null, FocasStatusMapper.BadCommunicationError, null, now);
return field switch
{
"CurrentTool" => new DataValueSnapshot(snap.CurrentTool, FocasStatusMapper.Good,
device.LastToolingUtc, now),
_ => new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now),
};
}
private DataValueSnapshot ReadOffsetField(string hostAddress, string slot, string axis, DateTime now)
{
if (!_devices.TryGetValue(hostAddress, out var device))
return new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now);
if (device.LastWorkOffsets is not { } snap)
return new DataValueSnapshot(null, FocasStatusMapper.BadCommunicationError, null, now);
var match = snap.Offsets.FirstOrDefault(o =>
string.Equals(o.Name, slot, StringComparison.OrdinalIgnoreCase));
if (match is null)
return new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now);
var value = axis switch
{
"X" => (double?)match.X,
"Y" => match.Y,
"Z" => match.Z,
_ => null,
};
if (value is null)
return new DataValueSnapshot(null, FocasStatusMapper.BadNodeIdUnknown, null, now);
return new DataValueSnapshot(value.Value, FocasStatusMapper.Good,
device.LastWorkOffsetsUtc, now);
}
private void TransitionDeviceState(DeviceState state, HostState newState)
{
HostState old;
@@ -717,6 +888,23 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
public FocasOverrideInfo? LastOverride { get; set; }
public DateTime LastOverrideUtc { get; set; }
/// <summary>
/// Cached <c>cnc_rdtnum</c> snapshot — current tool number — refreshed on
/// every probe tick. Reads of <c>Tooling/CurrentTool</c> serve from this
/// cache so they don't pile extra wire traffic on top of user-driven
/// reads (issue #260).
/// </summary>
public FocasToolingInfo? LastTooling { get; set; }
public DateTime LastToolingUtc { get; set; }
/// <summary>
/// Cached <c>cnc_rdzofs(1..6)</c> snapshot — G54..G59 work-coordinate
/// offsets — refreshed on every probe tick. Reads of
/// <c>Offsets/{slot}/{X|Y|Z}</c> serve from this cache (issue #260).
/// </summary>
public FocasWorkOffsetsInfo? LastWorkOffsets { get; set; }
public DateTime LastWorkOffsetsUtc { get; set; }
public void DisposeClient()
{
Client?.Dispose();

62
src/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FwlibFocasClient.cs
View File

@@ -242,6 +242,68 @@ internal sealed class FwlibFocasClient : IFocasClient
return TryReadInt16Param(param.Value, out var v) ? v : null;
}
public Task<FocasToolingInfo?> GetToolingAsync(CancellationToken cancellationToken)
{
if (!_connected) return Task.FromResult<FocasToolingInfo?>(null);
var buf = new FwlibNative.IODBTNUM();
var ret = FwlibNative.RdToolNumber(_handle, ref buf);
if (ret != 0) return Task.FromResult<FocasToolingInfo?>(null);
// FWLIB returns long; clamp to short for the surfaced Int16 (T-codes
// overflowing 32767 are vanishingly rare on Fanuc tool tables).
var t = buf.Data;
if (t > short.MaxValue) t = short.MaxValue;
else if (t < short.MinValue) t = short.MinValue;
return Task.FromResult<FocasToolingInfo?>(new FocasToolingInfo((short)t));
}
public Task<FocasWorkOffsetsInfo?> GetWorkOffsetsAsync(CancellationToken cancellationToken)
{
if (!_connected) return Task.FromResult<FocasWorkOffsetsInfo?>(null);
// 1..6 = G54..G59. Extended G54.1 P1..P48 use cnc_rdzofsr and are deferred.
// Pass axis=-1 so FWLIB fills every axis it has; we read the first 3 (X/Y/Z).
// Length = 4-byte header + 3 axes * 10-byte OFSB = 34. We request 4 + 8*10 = 84
// (the buffer ceiling) so a CNC with more axes still completes the call.
var slots = new List<FocasWorkOffset>(6);
string[] names = ["G54", "G55", "G56", "G57", "G58", "G59"];
for (short n = 1; n <= 6; n++)
{
var buf = new FwlibNative.IODBZOFS { Data = new byte[80] };
var ret = FwlibNative.RdWorkOffset(_handle, n, axis: -1, length: 4 + 8 * 10, ref buf);
if (ret != 0)
{
// Best-effort — a single-slot failure leaves the slot at 0.0; the cache
// still publishes so reads on the other offsets serve Good. The probe
// loop will retry on the next tick.
slots.Add(new FocasWorkOffset(names[n - 1], 0, 0, 0));
continue;
}
slots.Add(new FocasWorkOffset(
Name: names[n - 1],
X: DecodeOfsbAxis(buf.Data, axisIndex: 0),
Y: DecodeOfsbAxis(buf.Data, axisIndex: 1),
Z: DecodeOfsbAxis(buf.Data, axisIndex: 2)));
}
return Task.FromResult<FocasWorkOffsetsInfo?>(new FocasWorkOffsetsInfo(slots));
}
/// <summary>
/// Decode one OFSB axis block from a <c>cnc_rdzofs</c> data buffer. Each axis
/// occupies 10 bytes per <c>fwlib32.h</c>: <c>int data</c> + <c>short dec</c> +
/// <c>short unit</c> + <c>short disp</c>. The user-facing offset is
/// <c>data / 10^dec</c> — same convention as <c>cnc_rdmacro</c>.
/// </summary>
internal static double DecodeOfsbAxis(byte[] data, int axisIndex)
{
const int blockSize = 10;
var offset = axisIndex * blockSize;
if (offset + blockSize > data.Length) return 0;
var raw = BinaryPrimitives.ReadInt32LittleEndian(data.AsSpan(offset, 4));
var dec = BinaryPrimitives.ReadInt16LittleEndian(data.AsSpan(offset + 4, 2));
if (dec < 0 || dec > 9) dec = 0;
return raw / Math.Pow(10.0, dec);
}
// ---- PMC ----
private (object? value, uint status) ReadPmc(FocasAddress address, FocasDataType type)

61
src/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/FwlibNative.cs
View File

@@ -112,6 +112,35 @@ internal static class FwlibNative
[DllImport(Library, EntryPoint = "cnc_modal", ExactSpelling = true)]
public static extern short Modal(ushort handle, short type, short block, ref ODBMDL buffer);
// ---- Tooling ----
/// <summary>
/// <c>cnc_rdtnum</c> — read the currently selected tool number. Returns
/// <c>EW_OK</c> + populates <see cref="IODBTNUM.Data"/> with the active T-code.
/// Tool life + current offset index reads (<c>cnc_rdtlinfo</c>/<c>cnc_rdtlsts</c>/
/// <c>cnc_rdtofs</c>) are deferred per the F1-d plan — those calls use ODBTLIFE*
/// unions whose shape varies per series.
/// </summary>
[DllImport(Library, EntryPoint = "cnc_rdtnum", ExactSpelling = true)]
public static extern short RdToolNumber(ushort handle, ref IODBTNUM buffer);
// ---- Work coordinate offsets ----
/// <summary>
/// <c>cnc_rdzofs</c> — read one work-coordinate offset slot. <paramref name="number"/>:
/// 1..6 = G54..G59 (standard). Extended <c>G54.1 P1..P48</c> use <c>cnc_rdzofsr</c>
/// and are deferred. <paramref name="axis"/>: -1 = all axes returned, 1..N = single
/// axis. <paramref name="length"/>: 12 + (N axes * 8) — we request -1 and let FWLIB
/// fill up to <see cref="IODBZOFS.Data"/>'s 8-axis ceiling.
/// </summary>
[DllImport(Library, EntryPoint = "cnc_rdzofs", ExactSpelling = true)]
public static extern short RdWorkOffset(
ushort handle,
short number,
short axis,
short length,
ref IODBZOFS buffer);
// ---- Structs ----
/// <summary>
@@ -180,6 +209,38 @@ internal static class FwlibNative
public byte[] Data;
}
/// <summary>
/// IODBTNUM — current tool number read buffer. <see cref="Data"/> holds the active
/// T-code (Fanuc reference uses <c>long</c>; we narrow to <c>short</c> on the
/// managed side because <see cref="FocasToolingInfo.CurrentTool"/> surfaces as
/// <c>Int16</c>). Issue #260, F1-d.
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct IODBTNUM
{
public short Datano;
public short Type;
public int Data;
}
/// <summary>
/// IODBZOFS — work-coordinate offset read buffer. 4-byte header + per-axis
/// <c>OFSB</c> blocks (8 bytes each: 4-byte signed integer <c>data</c> + 2-byte
/// <c>dec</c> decimal-point count + 2-byte <c>unit</c> + 2-byte <c>disp</c>).
/// We marshal a fixed ceiling of 8 axes (= 64 bytes); the managed side reads
/// only the first 3 (X / Y / Z) per the F1-d effort sizing.
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct IODBZOFS
{
public short Datano;
public short Type;
// Up to 8 axes * 8 bytes per OFSB = 64 bytes. Each block: int data, short dec,
// short unit, short disp (10 bytes per fwlib32.h). We size for the worst case.
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 80)]
public byte[] Data;
}
/// <summary>ODBST — CNC status info. Machine state, alarm flags, automatic / edit mode.</summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ODBST

51
src/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/IFocasClient.cs
View File

@@ -91,6 +91,29 @@ public interface IFocasClient : IDisposable
Task<FocasOverrideInfo?> GetOverrideAsync(
FocasOverrideParameters parameters, CancellationToken cancellationToken)
=> Task.FromResult<FocasOverrideInfo?>(null);
/// <summary>
/// Read the current tool number via <c>cnc_rdtnum</c>. Surfaced on the FOCAS driver's
/// <c>Tooling/</c> fixed-tree per device (issue #260). Tool life + current offset
/// index are deferred — <c>cnc_rdtlinfo</c>/<c>cnc_rdtlsts</c> vary heavily across
/// CNC series + the FWLIB <c>ODBTLIFE*</c> unions need per-series shape handling
/// that exceeds the L-sized scope of this PR. Returns <c>null</c> when the wire
/// client cannot supply the snapshot (e.g. older transport variant).
/// </summary>
Task<FocasToolingInfo?> GetToolingAsync(CancellationToken cancellationToken)
=> Task.FromResult<FocasToolingInfo?>(null);
/// <summary>
/// Read the standard G54..G59 work-coordinate offsets via
/// <c>cnc_rdzofs(handle, n=1..6)</c>. Returns one <see cref="FocasWorkOffset"/>
/// per slot (issue #260). Extended G54.1 P1..P48 offsets are deferred — they use
/// a different FOCAS call (<c>cnc_rdzofsr</c>) + different range handling. Each
/// offset surfaces a fixed X/Y/Z view; lathes/mills with extra rotational axes
/// have those columns reported as 0.0. Returns <c>null</c> when the wire client
/// cannot supply the snapshot.
/// </summary>
Task<FocasWorkOffsetsInfo?> GetWorkOffsetsAsync(CancellationToken cancellationToken)
=> Task.FromResult<FocasWorkOffsetsInfo?>(null);
}
/// <summary>
@@ -164,6 +187,34 @@ public sealed record FocasOverrideInfo(
short? Spindle,
short? Jog);
/// <summary>
/// Snapshot of the currently selected tool number (issue #260). Sourced from
/// <c>cnc_rdtnum</c>. The active offset index is deferred — most modern CNCs
/// interleave tool number and offset H/D codes through different FOCAS calls
/// (<c>cnc_rdtofs</c> against a specific slot) and the issue body permits
/// surfacing tool number alone in the first cut. Surfaced as <c>Int16</c> in
/// the OPC UA address space.
/// </summary>
public sealed record FocasToolingInfo(short CurrentTool);
/// <summary>
/// One work-coordinate offset slot (G54..G59). Three axis columns are surfaced
/// (X / Y / Z) — the issue body permits a fixed 3-axis view because lathes and
/// mills typically don't expose extended rotational offsets via the standard
/// <c>cnc_rdzofs</c> call. Extended <c>G54.1 Pn</c> offsets via <c>cnc_rdzofsr</c>
/// are deferred to a follow-up PR. Values surfaced as <c>Float64</c> in microns
/// converted to user units (the FWLIB <c>data</c> field is an integer + decimal-
/// point count, decoded the same way <c>cnc_rdmacro</c> values are).
/// </summary>
public sealed record FocasWorkOffset(string Name, double X, double Y, double Z);
/// <summary>
/// Snapshot of the six standard work-coordinate offsets (G54..G59). Refreshed on
/// the probe tick + served from the per-device cache by reads of the
/// <c>Offsets/{name}/{X|Y|Z}</c> fixed-tree nodes (issue #260).
/// </summary>
public sealed record FocasWorkOffsetsInfo(IReadOnlyList<FocasWorkOffset> Offsets);
/// <summary>Factory for <see cref="IFocasClient"/>s. One client per configured device.</summary>
public interface IFocasClientFactory
{