0b4fb83546
Reference implementation of the frozen IF-LOC v1.0 contract (meta/contracts/ble-device-interface.md): byte-accurate codec for the telemetry, capture-trigger, and capture-result frames, plus a LocatorSimulator that replays a canned locate session over a transport-agnostic loopback link. Unblocks App development without firmware (Risk R-3, SRS S3.1). Plain net9.0 (not a MAUI head) so it builds/runs headless under the QA gate without the Android/iOS workloads; qa-gate.sh auto-discovers *.Tests.csproj under app/tests/. Round-trip test asserts field-level fidelity (every SRS S3.1 telemetry + capture-result field), sentinels, endianness (incl. RFC 4122 big-endian pointId), the REJECTED path, and exactly-once capture correlation (no silent drop, SRS-LOG-7). 8/8 green. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
217 lines
10 KiB
C#
217 lines
10 KiB
C#
using System.Buffers.Binary;
|
||
|
||
namespace IfLoc.Sim;
|
||
|
||
// Byte-accurate codec for the frozen IF-LOC v1.0 frames.
|
||
// All multi-byte integers little-endian except pointId (RFC 4122 big-endian, §5.3).
|
||
|
||
/// <summary>§4 telemetry payload — the locate data dictionary.</summary>
|
||
public sealed record Telemetry
|
||
{
|
||
public uint LocatorUptimeMs { get; init; }
|
||
/// <summary>Depth in metres (resolution 0.01 m). Null = no depth (sentinel on wire).</summary>
|
||
public double? DepthMeters { get; init; }
|
||
/// <summary>Signal current in mA. Null = invalid.</summary>
|
||
public ushort? SignalCurrentMa { get; init; }
|
||
/// <summary>Frequency in Hz. Null = n/a.</summary>
|
||
public uint? FrequencyHz { get; init; }
|
||
public LocateMode Mode { get; init; } = LocateMode.Single;
|
||
public SignalType SignalType { get; init; } = SignalType.Active;
|
||
public byte GainDb { get; init; }
|
||
public byte SignalLevel { get; init; }
|
||
public byte DistortionQualityPct { get; init; }
|
||
public byte SignalDirection { get; init; }
|
||
public byte CompassAngleDeg { get; init; }
|
||
/// <summary>Guidance offset −1000..+1000 (negative = left). Null = n/a.</summary>
|
||
public short? GuidanceOffset { get; init; }
|
||
public WarningFlags Warnings { get; init; }
|
||
public UtilityType Utility { get; init; } = UtilityType.None;
|
||
public byte BatteryPercent { get; init; }
|
||
public StatusFlags Status { get; init; }
|
||
|
||
/// <summary>Encodes the 28-byte payload (frame offsets 4..31).</summary>
|
||
public byte[] EncodePayload()
|
||
{
|
||
var p = new byte[IfLoc.TelemetryPayloadLen];
|
||
var s = p.AsSpan();
|
||
BinaryPrimitives.WriteUInt32LittleEndian(s[0..], LocatorUptimeMs); // 4
|
||
BinaryPrimitives.WriteInt16LittleEndian(s[4..], DepthMeters is { } d ? (short)Math.Round(d * 100) : IfLoc.DepthNoValue); // 8
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[6..], SignalCurrentMa ?? IfLoc.CurrentInvalid); // 10
|
||
BinaryPrimitives.WriteUInt32LittleEndian(s[8..], FrequencyHz ?? IfLoc.FrequencyNa); // 12
|
||
s[12] = (byte)Mode; // 16
|
||
s[13] = (byte)SignalType; // 17
|
||
s[14] = GainDb; // 18
|
||
s[15] = SignalLevel; // 19
|
||
s[16] = DistortionQualityPct; // 20
|
||
s[17] = SignalDirection; // 21
|
||
s[18] = CompassAngleDeg; // 22
|
||
s[19] = 0; // 23 reserved
|
||
BinaryPrimitives.WriteInt16LittleEndian(s[20..], GuidanceOffset ?? IfLoc.GuidanceNa); // 24
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[22..], (ushort)Warnings); // 26
|
||
s[24] = (byte)Utility; // 28
|
||
s[25] = BatteryPercent; // 29
|
||
s[26] = (byte)Status; // 30
|
||
s[27] = 0; // 31 reserved
|
||
return p;
|
||
}
|
||
|
||
/// <summary>Decodes a 28-byte payload (frame offsets 4..31).</summary>
|
||
public static Telemetry DecodePayload(ReadOnlySpan<byte> p)
|
||
{
|
||
if (p.Length < IfLoc.TelemetryPayloadLen)
|
||
throw new ArgumentException($"telemetry payload must be {IfLoc.TelemetryPayloadLen} bytes");
|
||
short depth = BinaryPrimitives.ReadInt16LittleEndian(p[4..]);
|
||
ushort cur = BinaryPrimitives.ReadUInt16LittleEndian(p[6..]);
|
||
uint freq = BinaryPrimitives.ReadUInt32LittleEndian(p[8..]);
|
||
short guid = BinaryPrimitives.ReadInt16LittleEndian(p[20..]);
|
||
return new Telemetry
|
||
{
|
||
LocatorUptimeMs = BinaryPrimitives.ReadUInt32LittleEndian(p[0..]),
|
||
DepthMeters = depth == IfLoc.DepthNoValue ? null : depth / 100.0,
|
||
SignalCurrentMa = cur == IfLoc.CurrentInvalid ? null : cur,
|
||
FrequencyHz = freq == IfLoc.FrequencyNa ? null : freq,
|
||
Mode = (LocateMode)p[12],
|
||
SignalType = (SignalType)p[13],
|
||
GainDb = p[14],
|
||
SignalLevel = p[15],
|
||
DistortionQualityPct = p[16],
|
||
SignalDirection = p[17],
|
||
CompassAngleDeg = p[18],
|
||
GuidanceOffset = guid == IfLoc.GuidanceNa ? null : guid,
|
||
Warnings = (WarningFlags)BinaryPrimitives.ReadUInt16LittleEndian(p[22..]),
|
||
Utility = (UtilityType)p[24],
|
||
BatteryPercent = p[25],
|
||
Status = (StatusFlags)p[26],
|
||
};
|
||
}
|
||
|
||
/// <summary>Encodes a full 32-byte TELEMETRY frame (header + payload).</summary>
|
||
public byte[] EncodeFrame(ushort seq)
|
||
{
|
||
var f = new byte[IfLoc.TelemetryFrameLen];
|
||
Frames.WriteHeader(f, MessageType.Telemetry, seq);
|
||
EncodePayload().CopyTo(f.AsSpan(IfLoc.HeaderLen));
|
||
return f;
|
||
}
|
||
}
|
||
|
||
/// <summary>§5.1 capture-trigger frame (locator → app).</summary>
|
||
public sealed record CaptureTrigger
|
||
{
|
||
public ushort CaptureSeq { get; init; }
|
||
public TriggerType TriggerType { get; init; }
|
||
public required Telemetry Snapshot { get; init; }
|
||
|
||
public byte[] EncodeFrame(ushort seq)
|
||
{
|
||
var f = new byte[IfLoc.CaptureTriggerFrameLen];
|
||
Frames.WriteHeader(f, MessageType.CaptureTrigger, seq);
|
||
BinaryPrimitives.WriteUInt16LittleEndian(f.AsSpan(4), CaptureSeq); // 4
|
||
f[6] = (byte)TriggerType; // 6
|
||
f[7] = 0; // 7 reserved
|
||
Snapshot.EncodePayload().CopyTo(f.AsSpan(8)); // 8..35
|
||
return f;
|
||
}
|
||
|
||
public static CaptureTrigger DecodeFrame(ReadOnlySpan<byte> f)
|
||
{
|
||
Frames.Expect(f, MessageType.CaptureTrigger, IfLoc.CaptureTriggerFrameLen);
|
||
return new CaptureTrigger
|
||
{
|
||
CaptureSeq = BinaryPrimitives.ReadUInt16LittleEndian(f[4..]),
|
||
TriggerType = (TriggerType)f[6],
|
||
Snapshot = Telemetry.DecodePayload(f.Slice(8, IfLoc.TelemetryPayloadLen)),
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>§5.2 capture-result frame (app → locator).</summary>
|
||
public sealed record CaptureResult
|
||
{
|
||
public ushort CaptureSeq { get; init; }
|
||
public CaptureOutcome Outcome { get; init; }
|
||
public ReasonCode Reason { get; init; }
|
||
public FixType FixType { get; init; }
|
||
/// <summary>WGS84 latitude in degrees. Null = no position.</summary>
|
||
public double? Lat { get; init; }
|
||
public double? Lon { get; init; }
|
||
/// <summary>Orthometric height in metres. Null = no position.</summary>
|
||
public double? OrthometricHeightM { get; init; }
|
||
/// <summary>Horizontal RMS (1σ) in metres. Null = unknown.</summary>
|
||
public double? HrmsM { get; init; }
|
||
public double? VrmsM { get; init; }
|
||
/// <summary>Position epoch, UTC.</summary>
|
||
public DateTimeOffset? Utc { get; init; }
|
||
/// <summary>Stored point UUIDv7. All-zero on REJECTED.</summary>
|
||
public Guid PointId { get; init; }
|
||
|
||
public byte[] EncodeFrame(ushort seq)
|
||
{
|
||
var f = new byte[IfLoc.CaptureResultFrameLen];
|
||
var s = f.AsSpan();
|
||
Frames.WriteHeader(f, MessageType.CaptureResult, seq);
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[4..], CaptureSeq);
|
||
s[6] = (byte)Outcome;
|
||
s[7] = (byte)Reason;
|
||
s[8] = (byte)FixType;
|
||
s[9] = 0;
|
||
BinaryPrimitives.WriteInt32LittleEndian(s[10..], Lat is { } la ? (int)Math.Round(la * 1e7) : IfLoc.PositionNoValue);
|
||
BinaryPrimitives.WriteInt32LittleEndian(s[14..], Lon is { } lo ? (int)Math.Round(lo * 1e7) : IfLoc.PositionNoValue);
|
||
BinaryPrimitives.WriteInt32LittleEndian(s[18..], OrthometricHeightM is { } h ? (int)Math.Round(h * 1000) : IfLoc.PositionNoValue);
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[22..], HrmsM is { } hr ? (ushort)Math.Round(hr * 1000) : IfLoc.RmsUnknown);
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[24..], VrmsM is { } vr ? (ushort)Math.Round(vr * 1000) : IfLoc.RmsUnknown);
|
||
BinaryPrimitives.WriteUInt16LittleEndian(s[26..], 0); // reserved
|
||
BinaryPrimitives.WriteUInt64LittleEndian(s[28..], Utc is { } t ? (ulong)t.ToUnixTimeMilliseconds() : 0);
|
||
PointId.TryWriteBytes(s.Slice(36, 16), bigEndian: true, out _); // §5.3 RFC 4122 network order
|
||
return f;
|
||
}
|
||
|
||
public static CaptureResult DecodeFrame(ReadOnlySpan<byte> f)
|
||
{
|
||
Frames.Expect(f, MessageType.CaptureResult, IfLoc.CaptureResultFrameLen);
|
||
int lat = BinaryPrimitives.ReadInt32LittleEndian(f[10..]);
|
||
int lon = BinaryPrimitives.ReadInt32LittleEndian(f[14..]);
|
||
int h = BinaryPrimitives.ReadInt32LittleEndian(f[18..]);
|
||
ushort hr = BinaryPrimitives.ReadUInt16LittleEndian(f[22..]);
|
||
ushort vr = BinaryPrimitives.ReadUInt16LittleEndian(f[24..]);
|
||
ulong ms = BinaryPrimitives.ReadUInt64LittleEndian(f[28..]);
|
||
return new CaptureResult
|
||
{
|
||
CaptureSeq = BinaryPrimitives.ReadUInt16LittleEndian(f[4..]),
|
||
Outcome = (CaptureOutcome)f[6],
|
||
Reason = (ReasonCode)f[7],
|
||
FixType = (FixType)f[8],
|
||
Lat = lat == IfLoc.PositionNoValue ? null : lat / 1e7,
|
||
Lon = lon == IfLoc.PositionNoValue ? null : lon / 1e7,
|
||
OrthometricHeightM = h == IfLoc.PositionNoValue ? null : h / 1000.0,
|
||
HrmsM = hr == IfLoc.RmsUnknown ? null : hr / 1000.0,
|
||
VrmsM = vr == IfLoc.RmsUnknown ? null : vr / 1000.0,
|
||
Utc = ms == 0 ? null : DateTimeOffset.FromUnixTimeMilliseconds((long)ms),
|
||
PointId = new Guid(f.Slice(36, 16), bigEndian: true),
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>Shared header helpers.</summary>
|
||
public static class Frames
|
||
{
|
||
public static void WriteHeader(Span<byte> f, MessageType type, ushort seq)
|
||
{
|
||
f[0] = IfLoc.FrameVersion;
|
||
f[1] = (byte)type;
|
||
BinaryPrimitives.WriteUInt16LittleEndian(f[2..], seq);
|
||
}
|
||
|
||
public static MessageType PeekType(ReadOnlySpan<byte> f) => (MessageType)f[1];
|
||
|
||
public static void Expect(ReadOnlySpan<byte> f, MessageType type, int fixedLen)
|
||
{
|
||
if (f.Length < fixedLen)
|
||
throw new ArgumentException($"{type} frame must be ≥ {fixedLen} bytes, got {f.Length}");
|
||
if (f[0] != IfLoc.FrameVersion)
|
||
throw new ArgumentException($"unsupported frameVersion 0x{f[0]:X2}");
|
||
if ((MessageType)f[1] != type)
|
||
throw new ArgumentException($"expected {type}, got 0x{f[1]:X2}");
|
||
}
|
||
}
|