fix(focas): scale positions from each cnc_rddynamic2 block; delete the fabricated cnc_getfigure
Every 0x0026 position block is self-describing: [i32 raw][u16 base][u16 exponent],
value = raw / base^exponent. The live 31i-B capture (ref-dynamic2-axis1.bin)
reports base 10 / exponent 4 on every block, so axis 1 is 280.1574 mm, not
2801574. The driver ignored the envelope and instead sent 'cnc_getfigure'
(0x00D3) -- a command that does not exist in the FOCAS wire protocol, served only
by the in-tree mock; the real control refuses it with rc=1, so on hardware every
position fell back to PositionDecimalPlaces (default 0) and published raw counts.
- ParsePositionBlock decodes the envelope into FocasPosition; the driver applies
raw / base^exponent at publish. PositionDecimalPlaces is now only the fallback
for a scale-less (4-byte) block.
- A block describing no position (0xFFFF vacant marker, base not 2/10, exponent
> 18 -- raw / 10^400 would read as a Good 0.0) publishes BadNoData on that node
alone; siblings are untouched.
- ReadPositionFiguresAsync / IFocasClient.GetPositionFiguresAsync and the
per-axis figure cache are deleted.
Ported from the sister ignitionoee FOCAS driver v0.17.0 (+ Focas.Wire-014);
live axis-scoped captures live-dynamic2-axis{1,2}-0x0026.bin (Z-34184 X 0.0374,
Y 370.177) added as fixtures.
This commit is contained in:
@@ -132,12 +132,11 @@ public sealed class FocasAlarmProjectionOptions
|
||||
/// default) to skip range validation and preserve legacy behaviour.
|
||||
/// </param>
|
||||
/// <param name="PositionDecimalPlaces">
|
||||
/// Axis positions returned by <c>cnc_rddynamic2</c> are scaled integers. The driver
|
||||
/// divides AbsolutePosition / MachinePosition / RelativePosition / DistanceToGo by
|
||||
/// <c>10^PositionDecimalPlaces</c> at the publish seam so they surface in engineering
|
||||
/// units on the Float64 axis nodes. Default <c>0</c> (no scaling) is byte-identical to
|
||||
/// legacy behaviour. Auto-fetching this via <c>cnc_getfigure</c> is deferred (wire-gated),
|
||||
/// so it is config-supplied. Negative values are clamped to 0 (no scaling).
|
||||
/// <b>Fallback only.</b> A real CNC's <c>cnc_rddynamic2</c> position block carries its own
|
||||
/// scale (<c>raw / base^exponent</c>), which the driver always applies and which wins over
|
||||
/// this setting. Only a block that carries no scale (a 4-byte payload, e.g. an older
|
||||
/// simulator) is divided by <c>10^PositionDecimalPlaces</c>. Default <c>0</c> (no scaling).
|
||||
/// Negative values are clamped to 0.
|
||||
/// </param>
|
||||
public sealed record FocasDeviceOptions(
|
||||
string HostAddress,
|
||||
|
||||
@@ -925,12 +925,6 @@ 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
|
||||
// (or a failed/empty figure read) 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), []);
|
||||
@@ -999,7 +993,7 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
var snap = await client.ReadDynamicAsync(axisIndex, ct).ConfigureAwait(false);
|
||||
// Keyed by the disambiguated segment, matching what DiscoverAsync emitted —
|
||||
// two same-named axes must not share (and overwrite) one cache slot (#548).
|
||||
PublishAxisSnapshot(state, cache.AxisSegments[i], snap, i); // i = 0-based axis index → PositionFigures lookup
|
||||
PublishAxisSnapshot(state, cache.AxisSegments[i], snap);
|
||||
if (i == 0) { firstAxisSnap = snap; PublishRateSnapshot(state, snap); }
|
||||
}
|
||||
|
||||
@@ -1102,40 +1096,34 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
/// <see cref="PollGroupEngine"/> → <see cref="ReadAsync"/> path, which hits
|
||||
/// <see cref="TryReadFixedTree"/> and returns these cached values.
|
||||
/// </summary>
|
||||
private static void PublishAxisSnapshot(DeviceState state, string axisSegment, FocasDynamicSnapshot snap, int axisIndex)
|
||||
private static void PublishAxisSnapshot(DeviceState state, string axisSegment, FocasDynamicSnapshot snap)
|
||||
{
|
||||
var host = state.Options.HostAddress;
|
||||
// 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). The managed WireFocasClient fetches figures live
|
||||
// via ReadPositionFiguresAsync; the config knob is the per-axis fallback only. FeedRate /
|
||||
// Each cnc_rddynamic2 position block carries its OWN scale (raw / base^exponent — see
|
||||
// FocasPosition), so positions surface in engineering units with no second read; the
|
||||
// configured PositionDecimalPlaces applies only to a block that carried no scale. A block
|
||||
// describing no usable position (vacant 0xFFFF marker, odd base, absurd exponent) is
|
||||
// cached as NaN and read back as BadNoData for that node alone (see NoValue). FeedRate /
|
||||
// SpindleSpeed (rate snapshot) and ServoLoad are NOT position-scaled (published elsewhere).
|
||||
var factor = AxisFactor(state, axisIndex);
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/AbsolutePosition")] = snap.AbsolutePosition / factor;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/MachinePosition")] = snap.MachinePosition / factor;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/RelativePosition")] = snap.RelativePosition / factor;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/DistanceToGo")] = snap.DistanceToGo / factor;
|
||||
var dp = state.Options.PositionDecimalPlaces;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/AbsolutePosition")] = snap.AbsolutePosition.ToEngineering(dp) ?? NoValue;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/MachinePosition")] = snap.MachinePosition.ToEngineering(dp) ?? NoValue;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/RelativePosition")] = snap.RelativePosition.ToEngineering(dp) ?? NoValue;
|
||||
state.LastFixedSnapshots[FixedTreeReference(host, $"Axes/{axisSegment}/DistanceToGo")] = snap.DistanceToGo.ToEngineering(dp) ?? NoValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Resolve the position-scale factor (10^figure) for a single axis. Auto
|
||||
/// (<c>cnc_getfigure</c>) wins per-axis; manual <c>PositionDecimalPlaces</c> 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 fetches figures live
|
||||
/// via <c>ReadPositionFiguresAsync</c>; the fallback branch fires only when the CNC
|
||||
/// reports no figure for a given axis (or the figure read returns empty).
|
||||
/// Cache sentinel for "the CNC answered, but this field holds no usable value" (an
|
||||
/// undescribable position / meter record). The read path turns it into
|
||||
/// <see cref="FocasStatusMapper.BadNoData"/> for that node only — never a Good number.
|
||||
/// </summary>
|
||||
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 const double NoValue = double.NaN;
|
||||
|
||||
/// <summary>Builds the snapshot for one cached double, mapping <see cref="NoValue"/> to BadNoData.</summary>
|
||||
private static DataValueSnapshot CachedDouble(double value, uint status, DateTime sourceUtc, DateTime now) =>
|
||||
double.IsNaN(value)
|
||||
? new DataValueSnapshot(null, status == FocasStatusMapper.Good ? FocasStatusMapper.BadNoData : status, sourceUtc, now)
|
||||
: new DataValueSnapshot(value, status, sourceUtc, now);
|
||||
|
||||
private static void PublishRateSnapshot(DeviceState state, FocasDynamicSnapshot snap)
|
||||
{
|
||||
@@ -1241,7 +1229,7 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
|
||||
if (state.LastFixedSnapshots.TryGetValue(
|
||||
FixedTreeReference(state.Options.HostAddress, path), out var raw))
|
||||
return new DataValueSnapshot(raw, status, sourceUtc, now);
|
||||
return CachedDouble(raw, status, sourceUtc, now);
|
||||
|
||||
// Servo-load match: path shape is "Axes/{name}/ServoLoad"
|
||||
var suffixFull = path;
|
||||
@@ -1249,7 +1237,7 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
{
|
||||
var axisName = suffixFull["Axes/".Length..^"/ServoLoad".Length];
|
||||
if (state.LastServoLoads.TryGetValue(axisName, out var load))
|
||||
return new DataValueSnapshot(load, status, sourceUtc, now);
|
||||
return CachedDouble(load, status, sourceUtc, now);
|
||||
}
|
||||
|
||||
// Spindle matches: "Spindle/{name}/Load" + "Spindle/{name}/MaxRpm"
|
||||
@@ -1578,9 +1566,10 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
public DateTime? LastFixedPollUtc { get; set; }
|
||||
/// <summary>
|
||||
/// Gets the last fixed-tree snapshots by field name. Double-typed so axis
|
||||
/// positions can carry the <c>10^PositionDecimalPlaces</c> engineering-unit
|
||||
/// scale applied at <see cref="PublishAxisSnapshot"/>; integer fields
|
||||
/// (feed rate, spindle speed) widen to double on store.
|
||||
/// positions can carry the engineering-unit scale applied at
|
||||
/// <see cref="PublishAxisSnapshot"/>; integer fields (feed rate, spindle speed) widen
|
||||
/// to double on store. <see cref="NoValue"/> (NaN) marks a field the CNC described as
|
||||
/// holding no value.
|
||||
/// </summary>
|
||||
public ConcurrentDictionary<string, double> LastFixedSnapshots { get; } = new(StringComparer.OrdinalIgnoreCase);
|
||||
/// <summary>Gets or sets the last program information snapshot.</summary>
|
||||
@@ -1593,13 +1582,6 @@ public sealed class FocasDriver : IDriver, IReadable, IWritable, ITagDiscovery,
|
||||
public ConcurrentDictionary<string, double> LastServoLoads { get; } = new(StringComparer.OrdinalIgnoreCase);
|
||||
/// <summary>Gets the last spindle load percentages by spindle index.</summary>
|
||||
public ConcurrentDictionary<int, int> LastSpindleLoads { get; } = new();
|
||||
/// <summary>
|
||||
/// Gets or sets the per-axis position decimal-place figures fetched once at init via
|
||||
/// <c>cnc_getfigure</c> (parallel to the axis-name list; index = axis). An auto figure
|
||||
/// for an axis WINS over the configured <c>PositionDecimalPlaces</c>; an empty list (or
|
||||
/// a failed/empty figure read) makes every axis fall back to that config knob.
|
||||
/// </summary>
|
||||
public IReadOnlyList<int> PositionFigures { get; set; } = [];
|
||||
|
||||
/// <summary>Disposes the FOCAS client instance.</summary>
|
||||
public void DisposeClient()
|
||||
|
||||
@@ -290,9 +290,9 @@ public static class FocasDriverFactoryExtensions
|
||||
public string? Series { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the axis-position decimal places. <c>cnc_rddynamic2</c> returns
|
||||
/// positions as scaled integers; the driver divides by <c>10^PositionDecimalPlaces</c>
|
||||
/// so they surface in engineering units. Omitted / <c>0</c> = no scaling (legacy).
|
||||
/// Gets or sets the fallback axis-position decimal places, applied only to a
|
||||
/// <c>cnc_rddynamic2</c> position block that carries no scale of its own (a real CNC's
|
||||
/// block does, and its scale wins). Omitted / <c>0</c> = no scaling.
|
||||
/// </summary>
|
||||
public int? PositionDecimalPlaces { get; init; }
|
||||
}
|
||||
|
||||
@@ -167,16 +167,6 @@ public interface IFocasClient : IDisposable
|
||||
/// <param name="cancellationToken">The cancellation token.</param>
|
||||
/// <returns>Per-spindle maximum RPM values, indexed as per <see cref="GetSpindleLoadsAsync"/>.</returns>
|
||||
Task<IReadOnlyList<int>> GetSpindleMaxRpmsAsync(CancellationToken cancellationToken);
|
||||
|
||||
/// <summary>
|
||||
/// Read the per-axis position decimal-place figure via <c>cnc_getfigure</c>. The returned
|
||||
/// list is parallel to <see cref="GetAxisNamesAsync"/> (index = axis). An EMPTY list means the
|
||||
/// CNC/backend does not report figures — the driver then falls back to the configured
|
||||
/// <c>PositionDecimalPlaces</c>. Values are clamped non-negative.
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">The cancellation token.</param>
|
||||
/// <returns>Per-axis position decimal-place figures; empty when the CNC/backend doesn't report them.</returns>
|
||||
Task<IReadOnlyList<int>> GetPositionFiguresAsync(CancellationToken cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>One servo-meter entry — one axis's current load percentage.</summary>
|
||||
@@ -238,12 +228,6 @@ public sealed record FocasSpindleName(string Name, string Suffix1, string Suffix
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fast-poll bundle for one axis. Position values are scaled integers; the caller
|
||||
/// divides by <c>10^DecimalPlaces</c> to get the decimal value. DecimalPlaces is
|
||||
/// supplied by the caller — either via device config (<c>PositionDecimalPlaces</c>) or
|
||||
/// the per-axis <see cref="IFocasClient.GetPositionFiguresAsync"/> (<c>cnc_getfigure</c>) path.
|
||||
/// </summary>
|
||||
/// <summary>
|
||||
/// Program + operation-mode snapshot. Name is the currently-executing
|
||||
/// program filename (e.g. "O0001.NC"); ONumber is its Fanuc O-number (1-9999).
|
||||
@@ -277,6 +261,10 @@ public static class FocasOpMode
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fast-poll bundle for one axis (<c>cnc_rddynamic2</c>). Each position carries its own scale
|
||||
/// (see <see cref="FocasPosition"/>); the driver converts it to engineering units at publish.
|
||||
/// </summary>
|
||||
public sealed record FocasDynamicSnapshot(
|
||||
int AxisIndex,
|
||||
int AlarmFlags,
|
||||
@@ -285,10 +273,63 @@ public sealed record FocasDynamicSnapshot(
|
||||
int SequenceNumber,
|
||||
int ActualFeedRate,
|
||||
int ActualSpindleSpeed,
|
||||
int AbsolutePosition,
|
||||
int MachinePosition,
|
||||
int RelativePosition,
|
||||
int DistanceToGo);
|
||||
FocasPosition AbsolutePosition,
|
||||
FocasPosition MachinePosition,
|
||||
FocasPosition RelativePosition,
|
||||
FocasPosition DistanceToGo);
|
||||
|
||||
/// <summary>
|
||||
/// One <c>cnc_rddynamic2</c> position (command <c>0x0026</c>) as the CNC reported it. The 8-byte
|
||||
/// wire block is self-describing — <c>[0..3] i32 raw</c>, <c>[4..5] u16 base</c> (2 or 10),
|
||||
/// <c>[6..7] u16 exponent</c> (<c>0xFFFF</c> = the slot holds no valid position) — and the real
|
||||
/// coordinate is <c>raw / base^exponent</c>. Confirmed on a real FANUC 31i-B: every block of the
|
||||
/// <c>ref-dynamic2-axis1.bin</c> capture reports base 10 / exponent 4 (280.1574 mm, not
|
||||
/// 2801574). That envelope is why no separate figure read exists: the <c>cnc_getfigure</c>
|
||||
/// (0x00D3) this driver used to send is not a FOCAS wire command, and the 31i-B refuses it.
|
||||
/// </summary>
|
||||
/// <param name="Raw">The raw i32 position value.</param>
|
||||
/// <param name="Base">The block's scale base (only 2 and 10 are meaningful).</param>
|
||||
/// <param name="Exponent">The block's scale exponent.</param>
|
||||
/// <param name="HasScale">
|
||||
/// True when the block carried the base/exponent envelope. A scale-less block (a 4-byte payload
|
||||
/// from an older simulator, or a raw integer a test supplies) falls back to the device's
|
||||
/// configured <c>PositionDecimalPlaces</c>.
|
||||
/// </param>
|
||||
public readonly record struct FocasPosition(int Raw, int Base = 0, int Exponent = 0, bool HasScale = false)
|
||||
{
|
||||
/// <summary>
|
||||
/// Largest exponent that can describe a real coordinate. An i32 carries at most ten digits,
|
||||
/// so anything beyond is a misread block — and left unbounded, <c>raw / 10^400</c> is
|
||||
/// <c>raw / Infinity = 0.0</c>, a Good reading of a machine parked at the origin.
|
||||
/// </summary>
|
||||
public const int MaxExponent = 18;
|
||||
|
||||
/// <summary>
|
||||
/// False when the block says it holds no usable position: the <c>0xFFFF</c> vacant marker, a
|
||||
/// base other than 2 or 10, or an absurd exponent. A per-node quality decision — that node
|
||||
/// publishes Bad, its siblings are unaffected.
|
||||
/// </summary>
|
||||
public bool IsValid => !HasScale || (Exponent <= MaxExponent && Base is 2 or 10);
|
||||
|
||||
/// <summary>
|
||||
/// The position in engineering units: <c>Raw / Base^Exponent</c> when the block carried its
|
||||
/// own scale (the configured fallback is then ignored), else <c>Raw / 10^fallbackDecimalPlaces</c>.
|
||||
/// Null when <see cref="IsValid"/> is false.
|
||||
/// </summary>
|
||||
/// <param name="fallbackDecimalPlaces">The device's configured <c>PositionDecimalPlaces</c>.</param>
|
||||
/// <returns>The scaled position, or null when the block describes none.</returns>
|
||||
public double? ToEngineering(int fallbackDecimalPlaces)
|
||||
{
|
||||
if (!IsValid) return null;
|
||||
if (HasScale) return Exponent == 0 ? Raw : Raw / Math.Pow(Base, Exponent);
|
||||
var dp = Math.Max(0, fallbackDecimalPlaces);
|
||||
return dp > 0 ? Raw / Math.Pow(10, dp) : Raw;
|
||||
}
|
||||
|
||||
/// <summary>A bare integer is a scale-less position (legacy shape; tests and fakes).</summary>
|
||||
/// <param name="raw">The raw position value.</param>
|
||||
public static implicit operator FocasPosition(int raw) => new(raw);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// One active alarm surfaced by <see cref="IFocasClient.ReadAlarmsAsync"/>. Shape
|
||||
|
||||
@@ -106,10 +106,6 @@ public sealed class SynchronizedFocasClient : IFocasClient
|
||||
public Task<IReadOnlyList<int>> GetSpindleMaxRpmsAsync(CancellationToken cancellationToken) =>
|
||||
RunBoundedAsync(ct => _inner.GetSpindleMaxRpmsAsync(ct), cancellationToken);
|
||||
|
||||
/// <inheritdoc />
|
||||
public Task<IReadOnlyList<int>> GetPositionFiguresAsync(CancellationToken cancellationToken) =>
|
||||
RunBoundedAsync(ct => _inner.GetPositionFiguresAsync(ct), cancellationToken);
|
||||
|
||||
/// <summary>Disposes the inner client and the call gate.</summary>
|
||||
public void Dispose()
|
||||
{
|
||||
|
||||
@@ -339,21 +339,59 @@ public sealed class FocasWireClient : IAsyncDisposable, IDisposable
|
||||
var rc = AggregateRc(blocks);
|
||||
if (rc != 0) return new FocasResult<WireDynamic>(rc, null);
|
||||
|
||||
return new FocasResult<WireDynamic>(rc, ParseDynamic2(blocks));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aggregate the <c>cnc_rddynamic2</c> response blocks of one axis read into a
|
||||
/// <see cref="WireDynamic"/>. The four <c>0x0026</c> blocks are taken in request order
|
||||
/// (absolute / machine / relative / distance-to-go), each decoded with its own scale by
|
||||
/// <see cref="ParsePositionBlock"/>.
|
||||
/// </summary>
|
||||
/// <param name="blocks">The response blocks of one <see cref="ReadDynamic2Async"/> request.</param>
|
||||
/// <returns>The decoded fast-poll bundle.</returns>
|
||||
internal static WireDynamic ParseDynamic2(IReadOnlyList<ResponseBlock> blocks)
|
||||
{
|
||||
var programPayload = FindPayload(blocks, 0x001c);
|
||||
return new FocasResult<WireDynamic>(
|
||||
rc,
|
||||
new WireDynamic(
|
||||
ReadFirstInt32(blocks, 0x001a),
|
||||
programPayload.Length >= 4 ? ReadInt32(programPayload, 0) : 0,
|
||||
programPayload.Length >= 8 ? ReadInt32(programPayload, 4) : 0,
|
||||
ReadFirstInt32(blocks, 0x001d),
|
||||
ReadFirstInt32(blocks, 0x0024),
|
||||
ReadFirstInt32(blocks, 0x0025),
|
||||
new WireAxisPosition(
|
||||
ReadSelectorPosition(blocks, 0x0026, 0),
|
||||
ReadSelectorPosition(blocks, 0x0026, 1),
|
||||
ReadSelectorPosition(blocks, 0x0026, 2),
|
||||
ReadSelectorPosition(blocks, 0x0026, 3))));
|
||||
return new WireDynamic(
|
||||
ReadFirstInt32(blocks, 0x001a),
|
||||
programPayload.Length >= 4 ? ReadInt32(programPayload, 0) : 0,
|
||||
programPayload.Length >= 8 ? ReadInt32(programPayload, 4) : 0,
|
||||
ReadFirstInt32(blocks, 0x001d),
|
||||
ReadFirstInt32(blocks, 0x0024),
|
||||
ReadFirstInt32(blocks, 0x0025),
|
||||
new WireAxisPosition(
|
||||
ReadSelectorPosition(blocks, 0x0026, 0),
|
||||
ReadSelectorPosition(blocks, 0x0026, 1),
|
||||
ReadSelectorPosition(blocks, 0x0026, 2),
|
||||
ReadSelectorPosition(blocks, 0x0026, 3)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Decode one <c>cnc_rddynamic2</c> position block (0x0026). The 8-byte block is
|
||||
/// self-describing — <c>[0..3] i32 raw</c>, <c>[4..5] u16 base</c>, <c>[6..7] u16 exponent</c>,
|
||||
/// value = <c>raw / base^exponent</c> — pinned on a real FANUC 31i-B (every block of
|
||||
/// <c>ref-dynamic2-axis1.bin</c> reports base 10 / exponent 4). A 4..7-byte block carries no
|
||||
/// envelope and is returned scale-less (the driver then applies the configured
|
||||
/// <c>PositionDecimalPlaces</c>); a shorter one decodes as a scale-less 0, as before.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Validity (vacant <c>0xFFFF</c> exponent, a base other than 2/10, an absurd exponent) is a
|
||||
/// per-node quality decision made by <see cref="FocasPosition.IsValid"/>, not a fault here:
|
||||
/// raising would let one empty slot take out the whole poll. Ported from the sister
|
||||
/// ignitionoee driver v0.17.0 (<c>Dynamic2.scaledPosition</c>, Focas.Wire-014).
|
||||
/// </remarks>
|
||||
/// <param name="payload">The 0x0026 response-block payload.</param>
|
||||
/// <returns>The position with whatever scale the block carried.</returns>
|
||||
internal static FocasPosition ParsePositionBlock(byte[] payload)
|
||||
{
|
||||
if (payload.Length >= 8)
|
||||
return new FocasPosition(
|
||||
ReadInt32(payload, 0),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(4, 2)),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(6, 2)),
|
||||
HasScale: true);
|
||||
return new FocasPosition(payload.Length >= 4 ? ReadInt32(payload, 0) : 0);
|
||||
}
|
||||
|
||||
/// <summary>Read servo-meter load percentages via <c>cnc_rdsvmeter</c> (command <c>0x0056</c>).</summary>
|
||||
@@ -382,42 +420,6 @@ public sealed class FocasWireClient : IAsyncDisposable, IDisposable
|
||||
return new FocasResult<IReadOnlyList<WireServoMeter>>(rc, result);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Read the per-axis position decimal-place figures via <c>cnc_getfigure</c>
|
||||
/// (command <c>0x00d3</c>). The response payload is a self-delimiting sequence of
|
||||
/// big-endian <c>short</c> decimal-place counts — one per configured axis — which the
|
||||
/// driver applies as a <c>10^figure</c> scale on raw position reads.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// The <c>0x00d3</c> command id is co-designed with the in-tree <c>focas_mock</c> 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).
|
||||
/// </remarks>
|
||||
/// <param name="cancellationToken">Cancellation token for the read operation.</param>
|
||||
/// <param name="timeout">Optional per-call timeout override.</param>
|
||||
/// <param name="pathId">Optional path ID override; defaults to <see cref="PathId"/>.</param>
|
||||
/// <returns>The per-axis position decimal-place figures, wrapped in a <see cref="FocasResult{T}"/>.</returns>
|
||||
public async Task<FocasResult<IReadOnlyList<int>>> 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<IReadOnlyList<int>>(block.Rc, null);
|
||||
|
||||
var payload = block.Payload;
|
||||
var figures = new List<int>(payload.Length / 2);
|
||||
for (var offset = 0; offset + 2 <= payload.Length; offset += 2)
|
||||
figures.Add(ReadInt16(payload, offset));
|
||||
|
||||
return new FocasResult<IReadOnlyList<int>>(block.Rc, figures);
|
||||
}
|
||||
|
||||
/// <summary>Read per-spindle load percentages via <c>cnc_rdspload</c> (command <c>0x0040</c> with arg1=0).</summary>
|
||||
/// <param name="spindleSelector">Spindle selector; -1 selects all spindles.</param>
|
||||
/// <param name="cancellationToken">Cancellation token for the read operation.</param>
|
||||
@@ -1044,17 +1046,17 @@ public sealed class FocasWireClient : IAsyncDisposable, IDisposable
|
||||
return payload.Length >= 4 ? ReadInt32(payload, 0) : 0;
|
||||
}
|
||||
|
||||
private static int ReadSelectorPosition(IReadOnlyList<ResponseBlock> blocks, ushort command, int selectorIndex)
|
||||
private static FocasPosition ReadSelectorPosition(IReadOnlyList<ResponseBlock> blocks, ushort command, int selectorIndex)
|
||||
{
|
||||
var seen = 0;
|
||||
foreach (var block in blocks)
|
||||
{
|
||||
if (block.Command != command) continue;
|
||||
if (seen == selectorIndex)
|
||||
return block.Payload.Length >= 4 ? ReadInt32(block.Payload, 0) : 0;
|
||||
return ParsePositionBlock(block.Payload);
|
||||
seen++;
|
||||
}
|
||||
return 0;
|
||||
return new FocasPosition(0);
|
||||
}
|
||||
|
||||
private static WireSysInfo ParseSysInfo(byte[] payload)
|
||||
|
||||
@@ -35,12 +35,15 @@ public sealed record WireStatus(
|
||||
short Edit,
|
||||
short TmMode);
|
||||
|
||||
/// <summary>Four-slot position quadruple for one axis: absolute / machine / relative / distance-to-go.</summary>
|
||||
/// <summary>
|
||||
/// Four-slot position quadruple for one axis: absolute / machine / relative / distance-to-go,
|
||||
/// each carrying the scale its own <c>0x0026</c> block reported.
|
||||
/// </summary>
|
||||
public sealed record WireAxisPosition(
|
||||
int Absolute,
|
||||
int Machine,
|
||||
int Relative,
|
||||
int Distance);
|
||||
FocasPosition Absolute,
|
||||
FocasPosition Machine,
|
||||
FocasPosition Relative,
|
||||
FocasPosition Distance);
|
||||
|
||||
/// <summary>
|
||||
/// Fast-poll bundle for one axis from <c>cnc_rddynamic2</c> — alarm flags, active program
|
||||
|
||||
@@ -181,6 +181,8 @@ public sealed class WireFocasClient : IFocasClient
|
||||
ThrowIfRcNonZero(result.Rc, "cnc_rddynamic2", result.IsOk);
|
||||
var d = result.Value!;
|
||||
var pos = d.Axis ?? new WireAxisPosition(0, 0, 0, 0);
|
||||
// Positions keep the scale their own 0x0026 block reported; the driver converts them to
|
||||
// engineering units at publish (FocasPosition.ToEngineering).
|
||||
return new FocasDynamicSnapshot(
|
||||
AxisIndex: axisIndex,
|
||||
AlarmFlags: d.Alarm,
|
||||
@@ -253,24 +255,6 @@ public sealed class WireFocasClient : IFocasClient
|
||||
public Task<IReadOnlyList<int>> GetSpindleMaxRpmsAsync(CancellationToken cancellationToken) =>
|
||||
ReadSpindleMetricAsync((sel, ct) => _wire.ReadSpindleMaxRpmAsync(sel, ct), cancellationToken);
|
||||
|
||||
/// <inheritdoc />
|
||||
// Reads via FocasWireClient.ReadPositionFiguresAsync; returns an empty list when the read
|
||||
// fails (non-zero RC — e.g. the series lacks cnc_getfigure) or is not connected. Never
|
||||
// throws — figure reads degrade to the configured PositionDecimalPlaces rather than faulting.
|
||||
public async Task<IReadOnlyList<int>> 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<IReadOnlyList<int>> ReadSpindleMetricAsync(
|
||||
Func<short, CancellationToken, Task<FocasResult<IReadOnlyList<WireSpindleMetric>>>> call,
|
||||
CancellationToken cancellationToken)
|
||||
|
||||
@@ -153,13 +153,6 @@ internal class FakeFocasClient : IFocasClient
|
||||
public virtual Task<IReadOnlyList<int>> GetSpindleMaxRpmsAsync(CancellationToken ct) =>
|
||||
Task.FromResult<IReadOnlyList<int>>([.. SpindleMaxRpms]);
|
||||
|
||||
/// <summary>Gets or sets the per-axis position decimal-place figures returned by <see cref="GetPositionFiguresAsync"/>.</summary>
|
||||
public IReadOnlyList<int> PositionFigures { get; set; } = [];
|
||||
/// <summary>Gets the per-axis position decimal-place figures asynchronously.</summary>
|
||||
/// <param name="ct">The cancellation token.</param>
|
||||
public virtual Task<IReadOnlyList<int>> GetPositionFiguresAsync(CancellationToken ct) =>
|
||||
Task.FromResult(PositionFigures);
|
||||
|
||||
/// <summary>Disposes the client.</summary>
|
||||
public virtual void Dispose()
|
||||
{
|
||||
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -1,164 +0,0 @@
|
||||
using Shouldly;
|
||||
using Xunit;
|
||||
using ZB.MOM.WW.OtOpcUa.Driver.FOCAS;
|
||||
|
||||
namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// Phase 4b — per-axis auto-scale from <c>cnc_getfigure</c> figures. The driver fetches
|
||||
/// per-axis position decimal-place figures at init and applies them at the
|
||||
/// <see cref="FocasDriver"/> publish seam. Precedence: an auto figure for an axis WINS;
|
||||
/// the configured <c>PositionDecimalPlaces</c> is the per-axis fallback when the CNC did
|
||||
/// not report a figure for that axis. These tests drive the Fake to exercise the driver's
|
||||
/// auto-scale-vs-fallback logic independently of the wire path.
|
||||
/// </summary>
|
||||
[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<int> 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<int> 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<object?> 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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// An auto figure for an axis WINS over the configured manual value: figure 3 ⇒ ÷10^3
|
||||
/// (12345 → 12.345), even though <c>PositionDecimalPlaces: 1</c> would have given ÷10.
|
||||
/// </summary>
|
||||
[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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// When <c>cnc_getfigure</c> reports nothing (empty list — the real wire backend's
|
||||
/// behaviour), the driver falls back to the configured <c>PositionDecimalPlaces</c>:
|
||||
/// manual 2 ⇒ ÷10^2 (12345 → 123.45).
|
||||
/// </summary>
|
||||
[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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Figures are applied per-axis independently: figures [3, 1] ⇒ axis 0 ÷10^3 and
|
||||
/// axis 1 ÷10^1.
|
||||
/// </summary>
|
||||
[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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The legacy manual-only path is unchanged: no auto figures + <c>PositionDecimalPlaces: 0</c>
|
||||
/// ⇒ factor 1.0, so the raw integer is published widened to double (12345 → 12345.0).
|
||||
/// </summary>
|
||||
[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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
using Shouldly;
|
||||
using Xunit;
|
||||
using ZB.MOM.WW.OtOpcUa.Core.Abstractions;
|
||||
using ZB.MOM.WW.OtOpcUa.Driver.FOCAS;
|
||||
|
||||
namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// Driver-level publish of self-describing <c>cnc_rddynamic2</c> positions. Each block's own
|
||||
/// <c>base^exponent</c> wins over the configured <c>PositionDecimalPlaces</c> (which is now only
|
||||
/// the fallback for a scale-less block), and a block describing no usable position Bads its own
|
||||
/// node without disturbing its siblings. Replaces the <c>cnc_getfigure</c> (0x00D3) auto-scale
|
||||
/// tests — that command is not a FOCAS wire command; the real 31i-B refuses it with <c>rc=1</c>.
|
||||
/// </summary>
|
||||
[Trait("Category", "Unit")]
|
||||
public sealed class FocasPositionBlockScaleTests
|
||||
{
|
||||
private const string Host = "focas://10.0.0.5:8193";
|
||||
|
||||
private static FocasDynamicSnapshot Snap(int axisIndex, FocasPosition abs, FocasPosition mach) =>
|
||||
new(AxisIndex: axisIndex, AlarmFlags: 0, ProgramNumber: 0, MainProgramNumber: 0,
|
||||
SequenceNumber: 0, ActualFeedRate: 0, ActualSpindleSpeed: 0,
|
||||
AbsolutePosition: abs, MachinePosition: mach, RelativePosition: 0, DistanceToGo: 0);
|
||||
|
||||
private static FocasDriver NewDriver(int positionDecimalPlaces, FakeFocasClient client) =>
|
||||
new(new FocasDriverOptions
|
||||
{
|
||||
Devices = [new FocasDeviceOptions(Host, PositionDecimalPlaces: positionDecimalPlaces)],
|
||||
Probe = new FocasProbeOptions { Enabled = false },
|
||||
FixedTree = new FocasFixedTreeOptions { Enabled = true, PollInterval = TimeSpan.FromMilliseconds(10) },
|
||||
}, "drv-1", new FakeFocasClientFactory { Customise = () => client });
|
||||
|
||||
private static async Task<DataValueSnapshot> ReadWhenPolledAsync(FocasDriver drv, string reference)
|
||||
{
|
||||
var deadline = DateTime.UtcNow + TimeSpan.FromSeconds(5);
|
||||
DataValueSnapshot last;
|
||||
do
|
||||
{
|
||||
// Until the first poll lands the reference resolves to nothing; once it has, the node
|
||||
// reads either Good or (for an undescribable block) BadNoData.
|
||||
last = (await drv.ReadAsync([reference], CancellationToken.None)).Single();
|
||||
if (last.StatusCode is FocasStatusMapper.Good or FocasStatusMapper.BadNoData) return last;
|
||||
await Task.Delay(15);
|
||||
} while (DateTime.UtcNow < deadline);
|
||||
return last;
|
||||
}
|
||||
|
||||
private static FakeFocasClient XAxisClient(FocasDynamicSnapshot snap)
|
||||
{
|
||||
var c = new FakeFocasClient();
|
||||
c.AxisNames.Clear();
|
||||
c.AxisNames.Add(new("X", ""));
|
||||
c.DynamicByAxis[1] = snap;
|
||||
return c;
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public async Task The_blocks_own_scale_wins_over_PositionDecimalPlaces()
|
||||
{
|
||||
// The live 31i-B shape: raw 2801574, base 10, exponent 4 → 280.1574 mm.
|
||||
var client = XAxisClient(Snap(1, new FocasPosition(2_801_574, 10, 4, HasScale: true), 0));
|
||||
var drv = NewDriver(positionDecimalPlaces: 1, client);
|
||||
await drv.InitializeAsync("{}", CancellationToken.None);
|
||||
|
||||
var abs = await ReadWhenPolledAsync(drv, $"{Host}/Axes/X/AbsolutePosition");
|
||||
|
||||
abs.StatusCode.ShouldBe(FocasStatusMapper.Good);
|
||||
((double)abs.Value!).ShouldBe(280.1574, 1e-9); // NOT 280157.4 (the ÷10 fallback)
|
||||
await drv.ShutdownAsync(CancellationToken.None);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public async Task A_scale_less_block_falls_back_to_PositionDecimalPlaces()
|
||||
{
|
||||
var client = XAxisClient(Snap(1, 12345, 0));
|
||||
var drv = NewDriver(positionDecimalPlaces: 2, client);
|
||||
await drv.InitializeAsync("{}", CancellationToken.None);
|
||||
|
||||
var abs = await ReadWhenPolledAsync(drv, $"{Host}/Axes/X/AbsolutePosition");
|
||||
|
||||
abs.StatusCode.ShouldBe(FocasStatusMapper.Good);
|
||||
abs.Value.ShouldBe(123.45);
|
||||
await drv.ShutdownAsync(CancellationToken.None);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public async Task A_vacant_block_bads_its_own_node_and_not_its_sibling()
|
||||
{
|
||||
var client = XAxisClient(Snap(
|
||||
1,
|
||||
abs: new FocasPosition(0, 10, 0xFFFF, HasScale: true), // vacant-slot marker
|
||||
mach: new FocasPosition(-22000, 10, 3, HasScale: true)));
|
||||
var drv = NewDriver(positionDecimalPlaces: 0, client);
|
||||
await drv.InitializeAsync("{}", CancellationToken.None);
|
||||
|
||||
var mach = await ReadWhenPolledAsync(drv, $"{Host}/Axes/X/MachinePosition");
|
||||
var abs = await ReadWhenPolledAsync(drv, $"{Host}/Axes/X/AbsolutePosition");
|
||||
|
||||
mach.StatusCode.ShouldBe(FocasStatusMapper.Good);
|
||||
mach.Value.ShouldBe(-22.0);
|
||||
abs.StatusCode.ShouldBe(FocasStatusMapper.BadNoData);
|
||||
abs.Value.ShouldBeNull("an undescribable block is not a Good 0.0 at the origin");
|
||||
await drv.ShutdownAsync(CancellationToken.None);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
using System.Buffers.Binary;
|
||||
using Shouldly;
|
||||
using Xunit;
|
||||
using ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Wire;
|
||||
|
||||
namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// <c>cnc_rddynamic2</c> position blocks (0x0026) are self-describing: <c>[0..3] i32 raw</c>,
|
||||
/// <c>[4..5] u16 base</c> (2 or 10), <c>[6..7] u16 exponent</c> (<c>0xFFFF</c> = vacant slot);
|
||||
/// <c>value = raw / base^exponent</c>. Ported from the sister ignitionoee FOCAS driver v0.17.0
|
||||
/// (which also deleted the fabricated <c>cnc_getfigure</c> 0x00D3 — the real 31i-B refuses it
|
||||
/// with <c>rc=1</c>) and Focas.Wire-014 (absurd exponents are refused, not divided by).
|
||||
/// </summary>
|
||||
[Trait("Category", "Unit")]
|
||||
public sealed class FocasPositionDecodeTests
|
||||
{
|
||||
private static byte[] Block(int raw, int @base, int exponent)
|
||||
{
|
||||
var b = new byte[8];
|
||||
BinaryPrimitives.WriteInt32BigEndian(b.AsSpan(0, 4), raw);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(b.AsSpan(4, 2), (ushort)@base);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(b.AsSpan(6, 2), (ushort)exponent);
|
||||
return b;
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void The_live_dynamic2_capture_scales_every_position_from_its_own_block()
|
||||
{
|
||||
// ref-dynamic2-axis1.bin: four 0x0026 blocks (absolute / machine / relative / distance of
|
||||
// axis 1), every one base 10, exponent 4. Undecoded they read 2801574 / 2801200 / 32 / 0.
|
||||
var dynamic = FocasWireClient.ParseDynamic2(WireFixtures.Blocks("ref-dynamic2-axis1.bin"));
|
||||
|
||||
dynamic.Axis.Absolute.ToEngineering(0)!.Value.ShouldBe(280.1574, 1e-9);
|
||||
dynamic.Axis.Machine.ToEngineering(0)!.Value.ShouldBe(280.1200, 1e-9);
|
||||
dynamic.Axis.Relative.ToEngineering(0)!.Value.ShouldBe(0.0032, 1e-9);
|
||||
dynamic.Axis.Distance.ToEngineering(0)!.Value.ShouldBe(0.0, 1e-9);
|
||||
dynamic.FeedRate.ShouldBe(0);
|
||||
dynamic.SpindleSpeed.ShouldBe(0);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void The_block_scale_wins_over_the_configured_decimal_places()
|
||||
{
|
||||
var p = FocasWireClient.ParsePositionBlock(Block(123456, 10, 3));
|
||||
p.ToEngineering(fallbackDecimalPlaces: 1)!.Value.ShouldBe(123.456, 1e-9);
|
||||
}
|
||||
|
||||
[Theory]
|
||||
[InlineData("live-dynamic2-axis1-0x0026.bin", 0.0374)] // Z-34184 X
|
||||
[InlineData("live-dynamic2-axis2-0x0026.bin", 370.177)] // Z-34184 Y
|
||||
public void Axis_scoped_live_requests_return_that_axis_scaled(string fixture, double expected)
|
||||
{
|
||||
var blocks = WireFixtures.Blocks(fixture).Where(b => b.Command == 0x0026).ToList();
|
||||
blocks.Count.ShouldBe(1, "an axis-scoped request returns exactly one 0x0026 block");
|
||||
|
||||
FocasWireClient.ParsePositionBlock(blocks[0].Payload).ToEngineering(0)!.Value.ShouldBe(expected, 1e-9);
|
||||
}
|
||||
|
||||
[Theory]
|
||||
[InlineData(40, 2, 3, 5.0)] // inch-mode binary base
|
||||
[InlineData(-22000, 10, 3, -22.0)]
|
||||
[InlineData(42, 10, 0, 42.0)]
|
||||
public void Positions_scale_by_base_to_the_exponent(int raw, int @base, int exponent, double expected)
|
||||
{
|
||||
FocasWireClient.ParsePositionBlock(Block(raw, @base, exponent)).ToEngineering(0)!.Value.ShouldBe(expected, 1e-9);
|
||||
}
|
||||
|
||||
[Theory]
|
||||
[InlineData(1, 10, 0xFFFF)] // vacant-slot marker
|
||||
[InlineData(1234, 7, 2)] // base neither 2 nor 10
|
||||
[InlineData(1234, 0, 2)]
|
||||
[InlineData(123456, 10, 400)] // would divide by Infinity → a Good 0.0 at the origin
|
||||
[InlineData(123456, 10, 40)]
|
||||
public void An_undescribable_block_has_no_value(int raw, int @base, int exponent)
|
||||
{
|
||||
var p = FocasWireClient.ParsePositionBlock(Block(raw, @base, exponent));
|
||||
p.IsValid.ShouldBeFalse();
|
||||
p.ToEngineering(3).ShouldBeNull();
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Every_plausible_exponent_still_decodes()
|
||||
{
|
||||
for (var exp = 0; exp <= 18; exp++)
|
||||
FocasWireClient.ParsePositionBlock(Block(1, 10, exp)).IsValid.ShouldBeTrue($"exponent {exp}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void A_block_without_a_scale_falls_back_to_the_configured_decimal_places()
|
||||
{
|
||||
// A 4-byte block carries no envelope (older simulators); only then does
|
||||
// PositionDecimalPlaces apply.
|
||||
var p = FocasWireClient.ParsePositionBlock([0x00, 0x00, 0x30, 0x39]); // 12345
|
||||
p.HasScale.ShouldBeFalse();
|
||||
p.ToEngineering(2)!.Value.ShouldBe(123.45, 1e-9);
|
||||
p.ToEngineering(0)!.Value.ShouldBe(12345.0);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void A_raw_integer_position_converts_implicitly_as_a_scale_less_value()
|
||||
{
|
||||
FocasPosition p = 12345;
|
||||
p.HasScale.ShouldBeFalse();
|
||||
p.Raw.ShouldBe(12345);
|
||||
}
|
||||
}
|
||||
@@ -5,12 +5,11 @@ using ZB.MOM.WW.OtOpcUa.Driver.FOCAS;
|
||||
namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// Phase 4 data-type tier — axis-position scaling. <c>cnc_rddynamic2</c> returns
|
||||
/// positions as scaled integers; the driver applies a <c>10^PositionDecimalPlaces</c>
|
||||
/// divide at the <see cref="FocasDriver"/> publish seam so positions surface in
|
||||
/// engineering units on the Float64 axis nodes. The managed wire backend fetches per-axis
|
||||
/// figures live via <c>cnc_getfigure</c>; the config knob is the per-axis fallback used
|
||||
/// when the CNC reports no figure for an axis. These tests cover the config-supplied path.
|
||||
/// Phase 4 data-type tier — axis-position scaling, config-supplied fallback path. A real
|
||||
/// CNC's <c>cnc_rddynamic2</c> position block carries its own <c>base^exponent</c> scale
|
||||
/// (covered by <see cref="FocasPositionBlockScaleTests"/>); a scale-less position — the
|
||||
/// raw integers these tests seed — is divided by <c>10^PositionDecimalPlaces</c> at the
|
||||
/// <see cref="FocasDriver"/> publish seam.
|
||||
/// </summary>
|
||||
[Trait("Category", "Unit")]
|
||||
public sealed class FocasPositionScalingTests
|
||||
|
||||
@@ -10,7 +10,7 @@ namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Tests;
|
||||
/// Offline byte-level coverage for the managed FOCAS/2 wire-protocol decode layer
|
||||
/// (<see cref="FocasWireProtocol"/>) — the big-endian framing + block-envelope decode that
|
||||
/// is this driver's analogue of P/Invoke struct marshalling. Before this suite the entire
|
||||
/// wire decode surface (including the <c>cnc_getfigure</c> figure stride) ran without a
|
||||
/// wire decode surface ran without a
|
||||
/// single byte-level test; every other test drives the <c>IFocasClient</c> seam through the
|
||||
/// Fake, which bypasses all framing. Recorded as Driver.FOCAS-013.
|
||||
/// </summary>
|
||||
@@ -200,29 +200,6 @@ public sealed class FocasWireProtocolTests
|
||||
Should.Throw<FocasWireException>(() => FocasWireProtocol.ParseResponseBlocks(body));
|
||||
}
|
||||
|
||||
// ---- The cnc_getfigure figure stride: decode a response block into shorts ----
|
||||
|
||||
[Fact]
|
||||
public void ParseResponseBlocks_yields_a_payload_that_decodes_as_a_sequence_of_figure_shorts()
|
||||
{
|
||||
// cnc_getfigure (command 0x00d3) returns one big-endian short per axis. Build the
|
||||
// response block and assert the payload decodes to the [3, 1, 0] figure sequence the
|
||||
// driver's auto-scale path reads.
|
||||
var figurePayload = new byte[6];
|
||||
BinaryPrimitives.WriteInt16BigEndian(figurePayload.AsSpan(0, 2), 3);
|
||||
BinaryPrimitives.WriteInt16BigEndian(figurePayload.AsSpan(2, 2), 1);
|
||||
BinaryPrimitives.WriteInt16BigEndian(figurePayload.AsSpan(4, 2), 0);
|
||||
|
||||
var body = BuildResponseBody((command: (ushort)0x00d3, rc: (short)0, payload: figurePayload));
|
||||
var block = FocasWireProtocol.ParseResponseBlocks(body).Single();
|
||||
|
||||
var figures = new List<int>();
|
||||
for (var offset = 0; offset + 2 <= block.Payload.Length; offset += 2)
|
||||
figures.Add(BinaryPrimitives.ReadInt16BigEndian(block.Payload.AsSpan(offset, 2)));
|
||||
|
||||
figures.ShouldBe(new[] { 3, 1, 0 });
|
||||
}
|
||||
|
||||
// ---- ReadAscii / ReadNameRecord ----
|
||||
|
||||
[Fact]
|
||||
|
||||
Reference in New Issue
Block a user