162 lines
7.4 KiB
Python
162 lines
7.4 KiB
Python
"""RPH sweep sources, both little endian.
|
|
|
|
E-502 collector PTY: whole-sweep RPH1 packets. The 48-byte header carries payload
|
|
size, source sweep/cycle IDs, monotonic capture window, input/output (65535 for
|
|
untagged) and dirty flag. Payload points are 8-byte ``marker, step, I, Q`` frames:
|
|
A for main and A8 for reference. Only complete queued packets may be dropped; a
|
|
packet already being written is always finished, so overflow cannot splice sweeps.
|
|
|
|
RFG board UDP (``SWEEP_AVG=1``): a ``u16 values_per_point, u16 q_shift`` header and
|
|
28-byte points ``u32 (sweep << 24) | step`` plus six int32 Q(q_shift) step averages
|
|
``[a1c0, a1c1, a1c2, a2c0, a2c1, a2c2]``. Channel k is ``a1ck + j*a2ck``; the main
|
|
signal is the synchronous detector ch0 - ch1 and the reference is ch2.
|
|
"""
|
|
|
|
from dataclasses import dataclass
|
|
import struct
|
|
|
|
import numpy as np
|
|
from numpy.typing import NDArray
|
|
|
|
MAIN_MARKER = 0x000A
|
|
REFERENCE_MARKER = 0x00A8
|
|
_HEADER = struct.Struct("<4sIQQQQHHI")
|
|
_POINT = struct.Struct("<HHhh")
|
|
_MAX_PAYLOAD_BYTES = 65534 * 2 * _POINT.size
|
|
_RFG_HEADER = struct.Struct("<HH")
|
|
_RFG_POINT = struct.Struct("<I6i")
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class RawSweep:
|
|
steps: NDArray[np.int32]
|
|
main: NDArray[np.complex64]
|
|
reference: NDArray[np.complex64]
|
|
source_id: int
|
|
cycle_id: int
|
|
capture_start_ns: int
|
|
capture_end_ns: int
|
|
combo: tuple[int, int] | None = None
|
|
dirty: bool = False
|
|
|
|
@property
|
|
def size(self) -> int:
|
|
return int(self.steps.size)
|
|
|
|
|
|
class RphRadarStreamParser:
|
|
"""Incremental packet parser; corrupt headers or point markers fail closed."""
|
|
|
|
def __init__(self) -> None:
|
|
self._buffer = bytearray()
|
|
self._aligned = False
|
|
|
|
def reset(self) -> None:
|
|
self._buffer.clear()
|
|
self._aligned = False
|
|
|
|
def feed(self, data: bytes) -> list[RawSweep]:
|
|
self._buffer.extend(data)
|
|
if not self._aligned:
|
|
index = self._buffer.find(b"RPH1")
|
|
if index < 0:
|
|
del self._buffer[:-3]
|
|
return []
|
|
del self._buffer[:index]
|
|
self._aligned = True
|
|
sweeps = []
|
|
while len(self._buffer) >= _HEADER.size:
|
|
magic, size, source_id, cycle_id, start, end, inp, out, flags = _HEADER.unpack_from(self._buffer)
|
|
if (magic != b"RPH1" or size > _MAX_PAYLOAD_BYTES or size % _POINT.size
|
|
or not source_id or not cycle_id or not 0 < start <= end
|
|
or flags & ~1 or (inp == 65535) != (out == 65535)):
|
|
raise ValueError("RadioPhotonic radar (RPH) protocol violation: invalid sweep header")
|
|
packet_size = _HEADER.size + size
|
|
if len(self._buffer) < packet_size:
|
|
break
|
|
main: dict[int, complex] = {}
|
|
reference: dict[int, complex] = {}
|
|
for marker, step, real, imag in _POINT.iter_unpack(self._buffer[_HEADER.size:packet_size]):
|
|
if marker not in (MAIN_MARKER, REFERENCE_MARKER) or not 0 < step < 65535:
|
|
raise ValueError("RadioPhotonic radar (RPH) protocol violation: invalid point frame")
|
|
target = main if marker == MAIN_MARKER else reference
|
|
if step in target:
|
|
raise ValueError("RadioPhotonic radar (RPH) protocol violation: duplicate point")
|
|
target[step] = complex(real, imag)
|
|
del self._buffer[:packet_size]
|
|
shared = sorted(main.keys() & reference.keys())
|
|
if shared:
|
|
sweeps.append(RawSweep(
|
|
np.asarray(shared, dtype=np.int32),
|
|
np.asarray([main[step] for step in shared], dtype=np.complex64),
|
|
np.asarray([reference[step] for step in shared], dtype=np.complex64),
|
|
source_id, cycle_id, start, end,
|
|
None if inp == 65535 else (inp, out), bool(flags & 1),
|
|
))
|
|
return sweeps
|
|
|
|
|
|
class RfgSweepAssembler:
|
|
"""Collect RFG points into sweeps of exactly the laser task's ``steps``.
|
|
|
|
A sweep closes when the next sweep id appears. Ids advance by one per sweep, so an
|
|
id that is not ahead of the current one (mod 256) is a late point of an older
|
|
sweep and is ignored — unless 128 sweep periods passed, when any id is legitimate. It is kept only with steps ``0..steps-1`` in order: the
|
|
reference phase advances up to ~1.8 rad per step, so a lost, duplicate or
|
|
reordered point would break the unwrapped frequency axis. Arrival times are
|
|
kernel receive stamps (see the reader). Step k leaves the board
|
|
``(k + 1) * step_ns`` after SWEEP_START and arrives no earlier, so the lower
|
|
envelope of ``arrival - (k + 1) * step_ns`` is the sweep start.
|
|
"""
|
|
|
|
def __init__(self, steps: int, step_ns: int, period_ns: int) -> None:
|
|
self._steps, self._step_ns, self._period_ns = steps, step_ns, period_ns
|
|
self._sweep_id: int | None = None # newest sweep; its points may still be arriving
|
|
self._last_ns = 0
|
|
self._points: list[tuple[int, complex, complex, int]] = []
|
|
self._count = 0
|
|
|
|
def reset(self) -> None:
|
|
"""Drop the sweep in progress; its remaining points then form a dirty sweep."""
|
|
self._sweep_id, self._points = None, []
|
|
|
|
def feed(self, datagram: bytes, received_ns: int) -> list[RawSweep]:
|
|
if len(datagram) < _RFG_HEADER.size:
|
|
raise ValueError("RFG protocol violation: truncated datagram")
|
|
values, q_shift = _RFG_HEADER.unpack_from(datagram)
|
|
if values != 6 or q_shift > 30 or (len(datagram) - _RFG_HEADER.size) % _RFG_POINT.size:
|
|
raise ValueError("RFG protocol violation: unexpected datagram layout")
|
|
scale = 1.0 / (1 << q_shift)
|
|
sweeps = []
|
|
for word, a1c0, a1c1, a1c2, a2c0, a2c1, a2c2 in _RFG_POINT.iter_unpack(datagram[_RFG_HEADER.size:]):
|
|
if word >> 24 != self._sweep_id:
|
|
if (self._sweep_id is not None and not 0 < ((word >> 24) - self._sweep_id) % 256 < 128
|
|
and received_ns - self._last_ns < 128 * self._period_ns):
|
|
continue
|
|
sweeps.extend(self._close())
|
|
self._sweep_id = word >> 24
|
|
self._last_ns = received_ns
|
|
if len(self._points) <= self._steps: # A stuck id cannot grow the buffer; the sweep is dirty.
|
|
self._points.append((word & 0xFFFFFF, complex(a1c0 - a1c1, a2c0 - a2c1) * scale,
|
|
complex(a1c2, a2c2) * scale, received_ns))
|
|
return sweeps
|
|
|
|
def _close(self) -> list[RawSweep]:
|
|
points, self._points = self._points, []
|
|
if not points:
|
|
return []
|
|
steps = np.asarray([point[0] for point in points], dtype=np.int32)
|
|
starts = [received - (step + 1) * self._step_ns for step, _, _, received in points]
|
|
start = min(starts)
|
|
# Every point of one sweep implies the same start within batching/latency jitter; an
|
|
# 8-bit id reused after an outage (256 sweeps, ~5 s) would splice two sweeps otherwise.
|
|
spliced = max(starts) - start > self._steps * self._step_ns
|
|
self._count += 1
|
|
return [RawSweep(
|
|
steps,
|
|
np.asarray([point[1] for point in points], dtype=np.complex64),
|
|
np.asarray([point[2] for point in points], dtype=np.complex64),
|
|
self._count, self._count, start, start + self._steps * self._step_ns,
|
|
dirty=spliced or not np.array_equal(steps, np.arange(self._steps)),
|
|
)]
|