90 lines
3.5 KiB
Python
90 lines
3.5 KiB
Python
"""Whole-sweep RPH1 packets from the collector's PTY (little endian).
|
|
|
|
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. Thus overflow cannot splice two physical sweeps together.
|
|
"""
|
|
|
|
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
|
|
|
|
|
|
@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
|