"""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(" 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)), )]