dohertj2/lmxopcua
1
0
Fork 0
Code Issues 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

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)