feat(focas): add cnc_getfigure wire command + focas-mock handler
This commit is contained in:
@@ -385,6 +385,41 @@ public sealed class FocasWireClient : IAsyncDisposable, IDisposable
|
|||||||
return new FocasResult<IReadOnlyList<WireServoMeter>>(rc, result);
|
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>
|
||||||
|
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>
|
/// <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="spindleSelector">Spindle selector; -1 selects all spindles.</param>
|
||||||
/// <param name="cancellationToken">Cancellation token for the read operation.</param>
|
/// <param name="cancellationToken">Cancellation token for the read operation.</param>
|
||||||
|
|||||||
@@ -287,14 +287,26 @@ public sealed class WireFocasClient : IFocasClient
|
|||||||
/// <summary>Gets the per-axis position decimal-place figures via <c>cnc_getfigure</c>.</summary>
|
/// <summary>Gets the per-axis position decimal-place figures via <c>cnc_getfigure</c>.</summary>
|
||||||
/// <param name="cancellationToken">Cancellation token for the operation.</param>
|
/// <param name="cancellationToken">Cancellation token for the operation.</param>
|
||||||
/// <returns>
|
/// <returns>
|
||||||
/// An empty list — the managed FOCAS/2 Ethernet wire client (<see cref="FocasWireClient"/>)
|
/// The per-axis decimal-place figures read from the CNC via
|
||||||
/// does not currently expose the <c>cnc_getfigure</c> command, so this backend reports no
|
/// <see cref="FocasWireClient.ReadPositionFiguresAsync"/>, or an empty list when the read
|
||||||
/// per-axis figures. Per the <see cref="IFocasClient.GetPositionFiguresAsync"/> contract an
|
/// fails (non-zero RC — e.g. the series lacks <c>cnc_getfigure</c>) or is not connected.
|
||||||
/// empty list signals the driver to fall back to the configured <c>PositionDecimalPlaces</c>.
|
/// Per the <see cref="IFocasClient.GetPositionFiguresAsync"/> contract an empty list signals
|
||||||
/// Never throws — figure reads degrade to the config knob rather than faulting.
|
/// the driver to fall back to the configured <c>PositionDecimalPlaces</c>. Never throws —
|
||||||
|
/// figure reads degrade to the config knob rather than faulting.
|
||||||
/// </returns>
|
/// </returns>
|
||||||
public Task<IReadOnlyList<int>> GetPositionFiguresAsync(CancellationToken cancellationToken) =>
|
public async Task<IReadOnlyList<int>> GetPositionFiguresAsync(CancellationToken cancellationToken)
|
||||||
Task.FromResult<IReadOnlyList<int>>(Array.Empty<int>());
|
{
|
||||||
|
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(
|
private static async Task<IReadOnlyList<int>> ReadSpindleMetricAsync(
|
||||||
Func<short, CancellationToken, Task<FocasResult<IReadOnlyList<WireSpindleMetric>>>> call,
|
Func<short, CancellationToken, Task<FocasResult<IReadOnlyList<WireSpindleMetric>>>> call,
|
||||||
|
|||||||
+4
@@ -78,6 +78,10 @@ def make_default_state(profile: Mapping[str, Any]) -> dict[str, Any]:
|
|||||||
"acts": 3200,
|
"acts": 3200,
|
||||||
"acts2": [3200] + [0] * (max_spindles - 1),
|
"acts2": [3200] + [0] * (max_spindles - 1),
|
||||||
"axis_names": axis_names[:max_axis],
|
"axis_names": axis_names[:max_axis],
|
||||||
|
# Per-axis position decimal places served by cnc_getfigure (command 0x00d3).
|
||||||
|
# Default every configured axis to 0 ⇒ 10^0 = 1.0 scale, leaving existing
|
||||||
|
# position reads unchanged. mock_patch can override individual axes.
|
||||||
|
"position_figures": {name: 0 for name in axis_names[:max_axis]},
|
||||||
"spindle_names": spindle_names,
|
"spindle_names": spindle_names,
|
||||||
"parameters": {
|
"parameters": {
|
||||||
"6711": {"type": "long", "value": 1, "decimal": 0, "description": "example parameter"},
|
"6711": {"type": "long", "value": 1, "decimal": 0, "description": "example parameter"},
|
||||||
|
|||||||
+10
@@ -256,6 +256,8 @@ class FocasMockServer:
|
|||||||
return self._wire_spindle_metric(request_block)
|
return self._wire_spindle_metric(request_block)
|
||||||
if command == 0x56:
|
if command == 0x56:
|
||||||
return self._wire_servo_meter()
|
return self._wire_servo_meter()
|
||||||
|
if command == 0xD3:
|
||||||
|
return self._wire_position_figures()
|
||||||
if command == 0x57:
|
if command == 0x57:
|
||||||
mode = self.store.snapshot()["operation_mode"]
|
mode = self.store.snapshot()["operation_mode"]
|
||||||
return self._u16(int(mode.get("mode", 0) if isinstance(mode, dict) else mode))
|
return self._u16(int(mode.get("mode", 0) if isinstance(mode, dict) else mode))
|
||||||
@@ -381,6 +383,14 @@ class FocasMockServer:
|
|||||||
payload += self._name_record(name, 4)
|
payload += self._name_record(name, 4)
|
||||||
return bytes(payload)
|
return bytes(payload)
|
||||||
|
|
||||||
|
def _wire_position_figures(self) -> bytes:
|
||||||
|
state = self.store.snapshot()
|
||||||
|
figures = state.get("position_figures", {})
|
||||||
|
payload = bytearray()
|
||||||
|
for name in state["axis_names"]:
|
||||||
|
payload += self._u16(int(figures.get(name, 0)))
|
||||||
|
return bytes(payload)
|
||||||
|
|
||||||
def _wire_spindle_metric(self, request_block: bytes) -> bytes:
|
def _wire_spindle_metric(self, request_block: bytes) -> bytes:
|
||||||
metric = self._block_u32(request_block, 8)
|
metric = self._block_u32(request_block, 8)
|
||||||
spindle = self.store.snapshot()["spindle"]
|
spindle = self.store.snapshot()["spindle"]
|
||||||
|
|||||||
Reference in New Issue
Block a user