3c2c4f29ea
Closes #259 Adds Modal/ + Override/ fixed-tree subfolders per FOCAS device, mirroring the pattern established by Status/ (#257) and Production/ (#258): cached snapshots refreshed on the probe tick, served from cache on read, no extra wire traffic on top of user-driven tag reads. Modal/ surfaces the four universally-present aux modal codes M/S/T/B from cnc_modal(type=100..103) as Int16. **G-group decoding (groups 1..21) is deferred to a follow-up** — the FWLIB ODBMDL union differs per series + group and the issue body explicitly permits this scoping. Adds the cnc_modal P/Invoke + ODBMDL struct + a generic int16 cnc_rdparam helper so the follow-up can add G-groups without further wire-level scaffolding. Override/ surfaces Feed/Rapid/Spindle/Jog from cnc_rdparam at MTB-specific parameter numbers (FocasDeviceOptions.OverrideParameters; defaults to 30i: 6010/6011/6014/6015). Per-field nullable params let a deployment hide overrides their MTB doesn't wire up; passing OverrideParameters=null suppresses the entire Override/ subfolder for that device. 6 unit tests cover discovery shape, omitted Override folder when unconfigured, partial Override field selection, cached-snapshot reads (Modal + Override), BadCommunicationError before first refresh, and the FwlibFocasClient disconnected short-circuit.
434 lines
19 KiB
C#
434 lines
19 KiB
C#
using System.Buffers.Binary;
|
|
using System.Collections.Concurrent;
|
|
|
|
namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS;
|
|
|
|
/// <summary>
|
|
/// <see cref="IFocasClient"/> implementation backed by Fanuc's licensed
|
|
/// <c>Fwlib32.dll</c> via <see cref="FwlibNative"/> P/Invoke. The DLL is NOT shipped with
|
|
/// OtOpcUa; the deployment places it next to the server executable or on <c>PATH</c>
|
|
/// (per Fanuc licensing — see <c>docs/v2/focas-deployment.md</c>).
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// <para>Construction is licence-safe — .NET P/Invoke is lazy, so instantiating this class
|
|
/// does NOT load <c>Fwlib32.dll</c>. The DLL only loads on the first wire call (Connect /
|
|
/// Read / Write / Probe). When missing, those calls throw <see cref="DllNotFoundException"/>
|
|
/// which the driver surfaces as <c>BadCommunicationError</c> through the normal exception
|
|
/// mapping.</para>
|
|
///
|
|
/// <para>Session-scoped handle — <c>cnc_allclibhndl3</c> opens one FWLIB handle per CNC;
|
|
/// all PMC / parameter / macro reads on that device go through the same handle. Dispose
|
|
/// calls <c>cnc_freelibhndl</c>.</para>
|
|
/// </remarks>
|
|
internal sealed class FwlibFocasClient : IFocasClient
|
|
{
|
|
private ushort _handle;
|
|
private bool _connected;
|
|
|
|
// Per-PMC-byte RMW lock registry. Bit writes to the same byte get serialised so two
|
|
// concurrent bit updates don't lose one another's modification. Key = "{addrType}:{byteAddr}".
|
|
private readonly ConcurrentDictionary<string, SemaphoreSlim> _rmwLocks = new();
|
|
|
|
private SemaphoreSlim GetRmwLock(short addrType, int byteAddr) =>
|
|
_rmwLocks.GetOrAdd($"{addrType}:{byteAddr}", _ => new SemaphoreSlim(1, 1));
|
|
|
|
public bool IsConnected => _connected;
|
|
|
|
public Task ConnectAsync(FocasHostAddress address, TimeSpan timeout, CancellationToken cancellationToken)
|
|
{
|
|
if (_connected) return Task.CompletedTask;
|
|
|
|
var timeoutMs = (int)Math.Max(1, timeout.TotalMilliseconds);
|
|
var ret = FwlibNative.AllcLibHndl3(address.Host, (ushort)address.Port, timeoutMs, out var handle);
|
|
if (ret != 0)
|
|
throw new InvalidOperationException(
|
|
$"FWLIB cnc_allclibhndl3 failed with EW_{ret} connecting to {address}.");
|
|
_handle = handle;
|
|
_connected = true;
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
public Task<(object? value, uint status)> ReadAsync(
|
|
FocasAddress address, FocasDataType type, CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult<(object?, uint)>((null, FocasStatusMapper.BadCommunicationError));
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
return address.Kind switch
|
|
{
|
|
FocasAreaKind.Pmc => Task.FromResult(ReadPmc(address, type)),
|
|
FocasAreaKind.Parameter => Task.FromResult(ReadParameter(address, type)),
|
|
FocasAreaKind.Macro => Task.FromResult(ReadMacro(address)),
|
|
_ => Task.FromResult<(object?, uint)>((null, FocasStatusMapper.BadNotSupported)),
|
|
};
|
|
}
|
|
|
|
public async Task<uint> WriteAsync(
|
|
FocasAddress address, FocasDataType type, object? value, CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return FocasStatusMapper.BadCommunicationError;
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
return address.Kind switch
|
|
{
|
|
FocasAreaKind.Pmc when type == FocasDataType.Bit && address.BitIndex is int =>
|
|
await WritePmcBitAsync(address, Convert.ToBoolean(value), cancellationToken).ConfigureAwait(false),
|
|
FocasAreaKind.Pmc => WritePmc(address, type, value),
|
|
FocasAreaKind.Parameter => WriteParameter(address, type, value),
|
|
FocasAreaKind.Macro => WriteMacro(address, value),
|
|
_ => FocasStatusMapper.BadNotSupported,
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Read-modify-write one bit within a PMC byte. Acquires a per-byte semaphore so
|
|
/// concurrent bit writes against the same byte serialise and neither loses its update.
|
|
/// </summary>
|
|
private async Task<uint> WritePmcBitAsync(
|
|
FocasAddress address, bool newValue, CancellationToken cancellationToken)
|
|
{
|
|
var addrType = FocasPmcAddrType.FromLetter(address.PmcLetter ?? "") ?? (short)0;
|
|
var bit = address.BitIndex ?? 0;
|
|
if (bit is < 0 or > 7)
|
|
throw new InvalidOperationException(
|
|
$"PMC bit index {bit} out of range (0-7) for {address.Canonical}.");
|
|
|
|
var rmwLock = GetRmwLock(addrType, address.Number);
|
|
await rmwLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
// Read the parent byte.
|
|
var readBuf = new FwlibNative.IODBPMC { Data = new byte[40] };
|
|
var readRet = FwlibNative.PmcRdPmcRng(
|
|
_handle, addrType, FocasPmcDataType.Byte,
|
|
(ushort)address.Number, (ushort)address.Number, 8 + 1, ref readBuf);
|
|
if (readRet != 0) return FocasStatusMapper.MapFocasReturn(readRet);
|
|
|
|
var current = readBuf.Data[0];
|
|
var updated = newValue
|
|
? (byte)(current | (1 << bit))
|
|
: (byte)(current & ~(1 << bit));
|
|
|
|
// Write the updated byte.
|
|
var writeBuf = new FwlibNative.IODBPMC
|
|
{
|
|
TypeA = addrType,
|
|
TypeD = FocasPmcDataType.Byte,
|
|
DatanoS = (ushort)address.Number,
|
|
DatanoE = (ushort)address.Number,
|
|
Data = new byte[40],
|
|
};
|
|
writeBuf.Data[0] = updated;
|
|
|
|
var writeRet = FwlibNative.PmcWrPmcRng(_handle, 8 + 1, ref writeBuf);
|
|
return writeRet == 0 ? FocasStatusMapper.Good : FocasStatusMapper.MapFocasReturn(writeRet);
|
|
}
|
|
finally
|
|
{
|
|
rmwLock.Release();
|
|
}
|
|
}
|
|
|
|
public Task<bool> ProbeAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult(false);
|
|
var buf = new FwlibNative.ODBST();
|
|
var ret = FwlibNative.StatInfo(_handle, ref buf);
|
|
return Task.FromResult(ret == 0);
|
|
}
|
|
|
|
public Task<FocasStatusInfo?> GetStatusAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult<FocasStatusInfo?>(null);
|
|
var buf = new FwlibNative.ODBST();
|
|
var ret = FwlibNative.StatInfo(_handle, ref buf);
|
|
if (ret != 0) return Task.FromResult<FocasStatusInfo?>(null);
|
|
return Task.FromResult<FocasStatusInfo?>(new FocasStatusInfo(
|
|
Dummy: buf.Dummy,
|
|
Tmmode: buf.TmMode,
|
|
Aut: buf.Aut,
|
|
Run: buf.Run,
|
|
Motion: buf.Motion,
|
|
Mstb: buf.Mstb,
|
|
EmergencyStop: buf.Emergency,
|
|
Alarm: buf.Alarm,
|
|
Edit: buf.Edit));
|
|
}
|
|
|
|
public Task<FocasProductionInfo?> GetProductionAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult<FocasProductionInfo?>(null);
|
|
if (!TryReadInt32Param(6711, out var produced) ||
|
|
!TryReadInt32Param(6712, out var required) ||
|
|
!TryReadInt32Param(6713, out var total))
|
|
{
|
|
return Task.FromResult<FocasProductionInfo?>(null);
|
|
}
|
|
// Cycle-time timer (type=2). Total seconds = minute*60 + msec/1000. Best-effort:
|
|
// a non-zero return leaves cycle-time at 0 rather than failing the whole snapshot
|
|
// — the parts counters are still useful even when cycle-time isn't supported.
|
|
var cycleSeconds = 0;
|
|
var tmrBuf = new FwlibNative.IODBTMR();
|
|
if (FwlibNative.RdTimer(_handle, type: 2, ref tmrBuf) == 0)
|
|
cycleSeconds = checked(tmrBuf.Minute * 60 + tmrBuf.Msec / 1000);
|
|
return Task.FromResult<FocasProductionInfo?>(new FocasProductionInfo(
|
|
PartsProduced: produced,
|
|
PartsRequired: required,
|
|
PartsTotal: total,
|
|
CycleTimeSeconds: cycleSeconds));
|
|
}
|
|
|
|
private bool TryReadInt32Param(ushort number, out int value)
|
|
{
|
|
var buf = new FwlibNative.IODBPSD { Data = new byte[32] };
|
|
var ret = FwlibNative.RdParam(_handle, number, axis: 0, length: 4 + 4, ref buf);
|
|
if (ret != 0) { value = 0; return false; }
|
|
value = BinaryPrimitives.ReadInt32LittleEndian(buf.Data);
|
|
return true;
|
|
}
|
|
|
|
private bool TryReadInt16Param(ushort number, out short value)
|
|
{
|
|
var buf = new FwlibNative.IODBPSD { Data = new byte[32] };
|
|
var ret = FwlibNative.RdParam(_handle, number, axis: 0, length: 4 + 2, ref buf);
|
|
if (ret != 0) { value = 0; return false; }
|
|
value = BinaryPrimitives.ReadInt16LittleEndian(buf.Data);
|
|
return true;
|
|
}
|
|
|
|
public Task<FocasModalInfo?> GetModalAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult<FocasModalInfo?>(null);
|
|
// type 100/101/102/103 = M/S/T/B (single auxiliary code, active modal block 0).
|
|
// Best-effort — if any single read fails we still surface the others as 0; the
|
|
// probe loop only updates the cache on a non-null return so a partial snapshot
|
|
// is preferable to throwing away every successful field.
|
|
return Task.FromResult<FocasModalInfo?>(new FocasModalInfo(
|
|
MCode: ReadModalAux(type: 100),
|
|
SCode: ReadModalAux(type: 101),
|
|
TCode: ReadModalAux(type: 102),
|
|
BCode: ReadModalAux(type: 103)));
|
|
}
|
|
|
|
private short ReadModalAux(short type)
|
|
{
|
|
var buf = new FwlibNative.ODBMDL { Data = new byte[8] };
|
|
var ret = FwlibNative.Modal(_handle, type, block: 0, ref buf);
|
|
if (ret != 0) return 0;
|
|
// For aux types (100..103) the union holds the code at offset 0 as a 2-byte
|
|
// value (<c>aux_data</c>). Reading as Int16 keeps the surface identical to the
|
|
// record contract; oversized values would have been truncated by FWLIB anyway.
|
|
return BinaryPrimitives.ReadInt16LittleEndian(buf.Data);
|
|
}
|
|
|
|
public Task<FocasOverrideInfo?> GetOverrideAsync(
|
|
FocasOverrideParameters parameters, CancellationToken cancellationToken)
|
|
{
|
|
if (!_connected) return Task.FromResult<FocasOverrideInfo?>(null);
|
|
// Each parameter is independently nullable — a null parameter number keeps the
|
|
// corresponding field at null + skips the wire call. A successful read on at
|
|
// least one parameter is enough to publish a snapshot; this matches the
|
|
// best-effort policy used by GetProductionAsync (issue #259).
|
|
var feed = TryReadOverride(parameters.FeedParam);
|
|
var rapid = TryReadOverride(parameters.RapidParam);
|
|
var spindle = TryReadOverride(parameters.SpindleParam);
|
|
var jog = TryReadOverride(parameters.JogParam);
|
|
return Task.FromResult<FocasOverrideInfo?>(new FocasOverrideInfo(feed, rapid, spindle, jog));
|
|
}
|
|
|
|
private short? TryReadOverride(ushort? param)
|
|
{
|
|
if (param is null) return null;
|
|
return TryReadInt16Param(param.Value, out var v) ? v : null;
|
|
}
|
|
|
|
// ---- PMC ----
|
|
|
|
private (object? value, uint status) ReadPmc(FocasAddress address, FocasDataType type)
|
|
{
|
|
var addrType = FocasPmcAddrType.FromLetter(address.PmcLetter ?? "")
|
|
?? throw new InvalidOperationException($"Unknown PMC letter '{address.PmcLetter}'.");
|
|
var dataType = FocasPmcDataType.FromFocasDataType(type);
|
|
var length = PmcReadLength(type);
|
|
|
|
var buf = new FwlibNative.IODBPMC { Data = new byte[40] };
|
|
var ret = FwlibNative.PmcRdPmcRng(
|
|
_handle, addrType, dataType,
|
|
(ushort)address.Number, (ushort)address.Number, (ushort)length, ref buf);
|
|
if (ret != 0) return (null, FocasStatusMapper.MapFocasReturn(ret));
|
|
|
|
var value = type switch
|
|
{
|
|
FocasDataType.Bit => ExtractBit(buf.Data[0], address.BitIndex ?? 0),
|
|
FocasDataType.Byte => (object)(sbyte)buf.Data[0],
|
|
FocasDataType.Int16 => (object)BinaryPrimitives.ReadInt16LittleEndian(buf.Data),
|
|
FocasDataType.Int32 => (object)BinaryPrimitives.ReadInt32LittleEndian(buf.Data),
|
|
FocasDataType.Float32 => (object)BinaryPrimitives.ReadSingleLittleEndian(buf.Data),
|
|
FocasDataType.Float64 => (object)BinaryPrimitives.ReadDoubleLittleEndian(buf.Data),
|
|
_ => (object)buf.Data[0],
|
|
};
|
|
return (value, FocasStatusMapper.Good);
|
|
}
|
|
|
|
private uint WritePmc(FocasAddress address, FocasDataType type, object? value)
|
|
{
|
|
var addrType = FocasPmcAddrType.FromLetter(address.PmcLetter ?? "") ?? (short)0;
|
|
var dataType = FocasPmcDataType.FromFocasDataType(type);
|
|
var length = PmcWriteLength(type);
|
|
|
|
var buf = new FwlibNative.IODBPMC
|
|
{
|
|
TypeA = addrType,
|
|
TypeD = dataType,
|
|
DatanoS = (ushort)address.Number,
|
|
DatanoE = (ushort)address.Number,
|
|
Data = new byte[40],
|
|
};
|
|
EncodePmcValue(buf.Data, type, value, address.BitIndex);
|
|
|
|
var ret = FwlibNative.PmcWrPmcRng(_handle, (ushort)length, ref buf);
|
|
return ret == 0 ? FocasStatusMapper.Good : FocasStatusMapper.MapFocasReturn(ret);
|
|
}
|
|
|
|
private (object? value, uint status) ReadParameter(FocasAddress address, FocasDataType type)
|
|
{
|
|
var buf = new FwlibNative.IODBPSD { Data = new byte[32] };
|
|
var length = ParamReadLength(type);
|
|
var ret = FwlibNative.RdParam(_handle, (ushort)address.Number, axis: 0, (short)length, ref buf);
|
|
if (ret != 0) return (null, FocasStatusMapper.MapFocasReturn(ret));
|
|
|
|
var value = type switch
|
|
{
|
|
FocasDataType.Bit when address.BitIndex is int bit => ExtractBit(buf.Data[0], bit),
|
|
FocasDataType.Byte => (object)(sbyte)buf.Data[0],
|
|
FocasDataType.Int16 => (object)BinaryPrimitives.ReadInt16LittleEndian(buf.Data),
|
|
FocasDataType.Int32 => (object)BinaryPrimitives.ReadInt32LittleEndian(buf.Data),
|
|
_ => (object)BinaryPrimitives.ReadInt32LittleEndian(buf.Data),
|
|
};
|
|
return (value, FocasStatusMapper.Good);
|
|
}
|
|
|
|
private uint WriteParameter(FocasAddress address, FocasDataType type, object? value)
|
|
{
|
|
var buf = new FwlibNative.IODBPSD
|
|
{
|
|
Datano = (short)address.Number,
|
|
Type = 0,
|
|
Data = new byte[32],
|
|
};
|
|
var length = ParamReadLength(type);
|
|
EncodeParamValue(buf.Data, type, value);
|
|
var ret = FwlibNative.WrParam(_handle, (short)length, ref buf);
|
|
return ret == 0 ? FocasStatusMapper.Good : FocasStatusMapper.MapFocasReturn(ret);
|
|
}
|
|
|
|
private (object? value, uint status) ReadMacro(FocasAddress address)
|
|
{
|
|
var buf = new FwlibNative.ODBM();
|
|
var ret = FwlibNative.RdMacro(_handle, (short)address.Number, length: 8, ref buf);
|
|
if (ret != 0) return (null, FocasStatusMapper.MapFocasReturn(ret));
|
|
|
|
// Macro value = mcr_val / 10^dec_val. Convert to double so callers get the correct
|
|
// scaled value regardless of the decimal-point count the CNC reports.
|
|
var scaled = buf.McrVal / Math.Pow(10.0, buf.DecVal);
|
|
return (scaled, FocasStatusMapper.Good);
|
|
}
|
|
|
|
private uint WriteMacro(FocasAddress address, object? value)
|
|
{
|
|
// Write as integer + 0 decimal places — callers that need decimal precision can extend
|
|
// this via a future WriteMacroScaled overload. Consistent with what most HMIs do today.
|
|
var intValue = Convert.ToInt32(value);
|
|
var ret = FwlibNative.WrMacro(_handle, (short)address.Number, length: 8, intValue, decimalPointCount: 0);
|
|
return ret == 0 ? FocasStatusMapper.Good : FocasStatusMapper.MapFocasReturn(ret);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
if (_connected)
|
|
{
|
|
try { FwlibNative.FreeLibHndl(_handle); } catch { }
|
|
_connected = false;
|
|
}
|
|
}
|
|
|
|
// ---- helpers ----
|
|
|
|
private static int PmcReadLength(FocasDataType type) => type switch
|
|
{
|
|
FocasDataType.Bit or FocasDataType.Byte => 8 + 1, // 8-byte header + 1 byte payload
|
|
FocasDataType.Int16 => 8 + 2,
|
|
FocasDataType.Int32 => 8 + 4,
|
|
FocasDataType.Float32 => 8 + 4,
|
|
FocasDataType.Float64 => 8 + 8,
|
|
_ => 8 + 1,
|
|
};
|
|
|
|
private static int PmcWriteLength(FocasDataType type) => PmcReadLength(type);
|
|
private static int ParamReadLength(FocasDataType type) => type switch
|
|
{
|
|
FocasDataType.Bit or FocasDataType.Byte => 4 + 1,
|
|
FocasDataType.Int16 => 4 + 2,
|
|
FocasDataType.Int32 => 4 + 4,
|
|
_ => 4 + 4,
|
|
};
|
|
|
|
private static bool ExtractBit(byte word, int bit) => (word & (1 << bit)) != 0;
|
|
|
|
internal static void EncodePmcValue(byte[] data, FocasDataType type, object? value, int? bitIndex)
|
|
{
|
|
switch (type)
|
|
{
|
|
case FocasDataType.Bit:
|
|
// PMC Bit writes with a non-null bitIndex go through WritePmcBitAsync's RMW path
|
|
// upstream. This branch only fires when a caller passes Bit with no bitIndex —
|
|
// treat the value as a whole-byte boolean (non-zero / zero).
|
|
data[0] = Convert.ToBoolean(value) ? (byte)1 : (byte)0;
|
|
break;
|
|
case FocasDataType.Byte:
|
|
data[0] = (byte)(sbyte)Convert.ToSByte(value);
|
|
break;
|
|
case FocasDataType.Int16:
|
|
BinaryPrimitives.WriteInt16LittleEndian(data, Convert.ToInt16(value));
|
|
break;
|
|
case FocasDataType.Int32:
|
|
BinaryPrimitives.WriteInt32LittleEndian(data, Convert.ToInt32(value));
|
|
break;
|
|
case FocasDataType.Float32:
|
|
BinaryPrimitives.WriteSingleLittleEndian(data, Convert.ToSingle(value));
|
|
break;
|
|
case FocasDataType.Float64:
|
|
BinaryPrimitives.WriteDoubleLittleEndian(data, Convert.ToDouble(value));
|
|
break;
|
|
default:
|
|
throw new NotSupportedException($"FocasDataType {type} not writable via PMC.");
|
|
}
|
|
_ = bitIndex; // bit-in-byte handled above
|
|
}
|
|
|
|
internal static void EncodeParamValue(byte[] data, FocasDataType type, object? value)
|
|
{
|
|
switch (type)
|
|
{
|
|
case FocasDataType.Byte:
|
|
data[0] = (byte)(sbyte)Convert.ToSByte(value);
|
|
break;
|
|
case FocasDataType.Int16:
|
|
BinaryPrimitives.WriteInt16LittleEndian(data, Convert.ToInt16(value));
|
|
break;
|
|
case FocasDataType.Int32:
|
|
BinaryPrimitives.WriteInt32LittleEndian(data, Convert.ToInt32(value));
|
|
break;
|
|
default:
|
|
BinaryPrimitives.WriteInt32LittleEndian(data, Convert.ToInt32(value));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>Default <see cref="IFocasClientFactory"/> — produces a fresh <see cref="FwlibFocasClient"/> per device.</summary>
|
|
public sealed class FwlibFocasClientFactory : IFocasClientFactory
|
|
{
|
|
public IFocasClient Create() => new FwlibFocasClient();
|
|
}
|