286 lines
13 KiB
Python
286 lines
13 KiB
Python
"""Background reader publishing the latest completed RadioPhotonic radar (RPH) sweep.
|
|
|
|
The source (E-502 collector TTY or RFG board UDP socket) emits sweeps continuously,
|
|
faster than callers invoke :meth:`RphRadarService.acquire`. A daemon thread drains
|
|
the descriptor non-stop, feeds it to :class:`RphRadarStreamParser` or
|
|
:class:`RfgSweepAssembler`, and stores the
|
|
most recent :class:`RawSweep` in a single-slot mailbox. :meth:`read_sweep`
|
|
returns the freshest sweep; if a newer one arrives before the consumer reads, it
|
|
overwrites the previous unread value — by design, since consumers always want the
|
|
latest data. Parser/stream errors are captured and re-raised on the consumer
|
|
thread (fail-fast; the supervisor relaunches a clean collector).
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from contextlib import suppress
|
|
from dataclasses import dataclass, field
|
|
import errno
|
|
import logging
|
|
import os
|
|
import select
|
|
import socket
|
|
import struct
|
|
import subprocess
|
|
import threading
|
|
import time
|
|
|
|
from python_app.hardware_full.rph_radar.protocol import RawSweep, RfgSweepAssembler, RphRadarStreamParser
|
|
from python_app.models.run_config_validation import parse_udp_bind
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
# Large reads keep up with bursty CDC-ACM/PTY writers without raising the syscall
|
|
# rate; 64 KiB matches the typical Linux PTY buffer size.
|
|
_READ_CHUNK_BYTES = 65536
|
|
# select() poll interval — short enough to react to close() promptly, long enough
|
|
# that idle CPU stays near zero.
|
|
_READ_POLL_INTERVAL_S = 0.1
|
|
# Kernel receive stamps (Linux SO_TIMESTAMPNS == SCM_TIMESTAMPNS on x86_64 and aarch64,
|
|
# a 64-bit timespec) are immune to the reader thread's scheduling stalls.
|
|
_SO_TIMESTAMPNS = 35
|
|
_TIMESPEC = struct.Struct("qq")
|
|
# A REALTIME step between the kernel stamp and the clock read would mistranslate it;
|
|
# NTP slewing moves the REALTIME-monotonic offset by microseconds only.
|
|
_CLOCK_JUMP_NS = 1_000_000
|
|
|
|
|
|
def raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None:
|
|
"""Raise if the external collector process has exited."""
|
|
if process is None:
|
|
return
|
|
return_code = process.poll()
|
|
if return_code is not None:
|
|
raise RuntimeError(f"RadioPhotonic radar (RPH) collector exited with code {return_code}")
|
|
|
|
|
|
def _clock_offset_ns() -> int:
|
|
return time.clock_gettime_ns(time.CLOCK_REALTIME) - time.monotonic_ns()
|
|
|
|
|
|
@dataclass(slots=True)
|
|
class RphRadarStreamReader:
|
|
"""Daemon-thread reader of the collector TTY, or of the RFG UDP stream when ``rfg`` is set."""
|
|
|
|
endpoint: str # TTY path, or HOST:PORT bind address for the RFG stream
|
|
assembler: RfgSweepAssembler | None = None
|
|
_socket: socket.socket | None = field(init=False, default=None, repr=False)
|
|
_clock_offset_ns: int | None = field(init=False, default=None, repr=False)
|
|
_fd: int | None = field(init=False, default=None, repr=False)
|
|
_thread: threading.Thread | None = field(init=False, default=None, repr=False)
|
|
_stop_event: threading.Event = field(init=False, default_factory=threading.Event, repr=False)
|
|
_mailbox_cv: threading.Condition = field(init=False, default_factory=threading.Condition, repr=False)
|
|
_latest_sweep: RawSweep | None = field(init=False, default=None, repr=False)
|
|
# Latest clean sweep per switch combination, for the switch-aware collector.
|
|
# read_sweep() ignores this and serves the single newest sweep (calibration /
|
|
# non-switch mode); an optional combo selects its own slot.
|
|
_combo_slots: dict[tuple[int, int], RawSweep] = field(init=False, default_factory=dict, repr=False)
|
|
_reader_error: Exception | None = field(init=False, default=None, repr=False)
|
|
_published_count: int = field(init=False, default=0, repr=False)
|
|
last_dirty_size: int | None = field(init=False, default=None)
|
|
|
|
def open(self) -> None:
|
|
"""Open the TTY or bind the UDP socket and start the background reader thread."""
|
|
if self._fd is not None:
|
|
return
|
|
if self.assembler is None:
|
|
self._fd = os.open(self.endpoint, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
|
|
else:
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
# Taken before bind, so a REALTIME step while the first datagrams queue is still seen.
|
|
offset_ns = _clock_offset_ns()
|
|
try:
|
|
# ~10k datagrams/s: a deep buffer rides out receiver stalls; the kernel caps it at rmem_max.
|
|
sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 4 << 20)
|
|
sock.setsockopt(socket.SOL_SOCKET, _SO_TIMESTAMPNS, 1)
|
|
sock.bind(parse_udp_bind(self.endpoint))
|
|
sock.setblocking(False)
|
|
except OSError:
|
|
sock.close()
|
|
raise
|
|
self._socket, self._fd, self._clock_offset_ns = sock, sock.fileno(), offset_ns
|
|
logger.info("RFG socket receive buffer: %d bytes", sock.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF))
|
|
self._stop_event.clear()
|
|
self._latest_sweep = None
|
|
self._combo_slots = {}
|
|
self._reader_error = None
|
|
self._published_count = 0
|
|
self._thread = threading.Thread(
|
|
target=self._reader_loop,
|
|
name=f"rph-radar-reader[{self.endpoint}]",
|
|
daemon=True,
|
|
)
|
|
try:
|
|
self._thread.start()
|
|
except Exception:
|
|
self._thread = None
|
|
self._close_descriptor()
|
|
raise
|
|
logger.info("RadioPhotonic radar (RPH) reader started on %s", self.endpoint)
|
|
|
|
def close(self) -> None:
|
|
"""Stop the reader thread and close the descriptor."""
|
|
logger.debug("Stopping RadioPhotonic radar (RPH) reader on %s", self.endpoint)
|
|
self._stop_event.set()
|
|
with self._mailbox_cv:
|
|
self._mailbox_cv.notify_all()
|
|
if self._thread is not None:
|
|
self._thread.join(timeout=1.0)
|
|
if self._thread.is_alive():
|
|
logger.warning("RadioPhotonic radar (RPH) reader thread did not stop within 1.0s")
|
|
self._thread = None
|
|
self._close_descriptor()
|
|
self._latest_sweep = None
|
|
self._combo_slots = {}
|
|
self._reader_error = None
|
|
|
|
def _close_descriptor(self) -> None:
|
|
fd, sock, self._fd, self._socket = self._fd, self._socket, None, None
|
|
if sock is not None:
|
|
sock.close()
|
|
elif fd is not None:
|
|
os.close(fd)
|
|
|
|
@property
|
|
def published_count(self) -> int:
|
|
"""Total number of sweeps the reader thread has produced."""
|
|
with self._mailbox_cv:
|
|
return self._published_count
|
|
|
|
def read_sweep(
|
|
self,
|
|
*,
|
|
timeout_s: float,
|
|
combo: tuple[int, int] | None = None,
|
|
process: subprocess.Popen[bytes] | None = None,
|
|
) -> RawSweep:
|
|
"""Wait for and return the next published sweep.
|
|
|
|
Raises :class:`TimeoutError` if none arrives within ``timeout_s``,
|
|
:class:`RuntimeError` if the collector process exited, and re-raises any
|
|
error caught by the reader thread.
|
|
"""
|
|
if self._thread is None:
|
|
raise RuntimeError("RadioPhotonic radar (RPH) reader is not open")
|
|
deadline = time.monotonic() + float(timeout_s)
|
|
with self._mailbox_cv:
|
|
while True:
|
|
if self._stop_event.is_set():
|
|
raise RuntimeError("RadioPhotonic radar (RPH) reader is closed")
|
|
# Deliver a pending sweep first: if the reader both published a
|
|
# sweep and then died, the consumer still sees the good data and
|
|
# only meets the error on the next call.
|
|
sweep = self._latest_sweep if combo is None else self._combo_slots.pop(combo, None)
|
|
if sweep is not None:
|
|
if combo is None:
|
|
self._latest_sweep = None
|
|
return sweep
|
|
if self._reader_error is not None:
|
|
raise self._reader_error
|
|
raise_if_process_exited(process)
|
|
remaining_s = deadline - time.monotonic()
|
|
if remaining_s <= 0.0:
|
|
rejected = "" if self.last_dirty_size is None else f"; last rejected sweep had {self.last_dirty_size} points"
|
|
raise TimeoutError(
|
|
f"Timed out waiting for RadioPhotonic radar (RPH) sweep after {float(timeout_s):.3f}s{rejected}"
|
|
)
|
|
self._mailbox_cv.wait(timeout=min(_READ_POLL_INTERVAL_S, remaining_s))
|
|
|
|
def _reader_loop(self) -> None:
|
|
"""Drain the descriptor, parse sweeps, and publish each completed one until stop."""
|
|
parser = RphRadarStreamParser()
|
|
try:
|
|
while not self._stop_event.is_set():
|
|
if self.assembler is None:
|
|
chunk = self._read_available()
|
|
sweeps = parser.feed(chunk) if chunk else []
|
|
else:
|
|
datagram = self._receive_datagram()
|
|
sweeps = self.assembler.feed(*datagram) if datagram is not None else []
|
|
for sweep in sweeps:
|
|
self._publish_sweep(sweep)
|
|
except Exception as exc: # noqa: BLE001 — surfaced to the consumer via read_sweep
|
|
logger.exception("RadioPhotonic radar (RPH) reader thread failed on %s", self.endpoint)
|
|
self._publish_error(exc)
|
|
|
|
def _read_available(self) -> bytes:
|
|
"""Block on ``select`` up to the poll interval; return new bytes (maybe empty).
|
|
|
|
Returns ``b""`` when no data is ready yet or a stop was requested; raises
|
|
:class:`RuntimeError` on stream close or an unrecoverable read error.
|
|
"""
|
|
fd = self._fd
|
|
if fd is None or self._stop_event.is_set():
|
|
return b""
|
|
try:
|
|
readable, _, _ = select.select([fd], [], [], _READ_POLL_INTERVAL_S)
|
|
except InterruptedError:
|
|
return b""
|
|
if not readable:
|
|
return b""
|
|
try:
|
|
chunk = os.read(fd, _READ_CHUNK_BYTES)
|
|
except BlockingIOError:
|
|
return b""
|
|
except OSError as exc:
|
|
if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}:
|
|
return b""
|
|
raise RuntimeError(f"Failed to read RadioPhotonic radar (RPH) source `{self.endpoint}`: {exc}") from exc
|
|
if not chunk:
|
|
raise RuntimeError(f"RadioPhotonic radar (RPH) source `{self.endpoint}` closed while reading")
|
|
return chunk
|
|
|
|
def _receive_datagram(self) -> tuple[bytes, int] | None:
|
|
"""Return one RFG datagram with its kernel receive time on the monotonic clock."""
|
|
sock = self._socket
|
|
if sock is None or self._stop_event.is_set():
|
|
return None
|
|
try:
|
|
readable, _, _ = select.select([sock], [], [], _READ_POLL_INTERVAL_S)
|
|
if not readable:
|
|
return None
|
|
datagram, ancillary, _, _ = sock.recvmsg(_READ_CHUNK_BYTES, socket.CMSG_SPACE(_TIMESPEC.size))
|
|
except (BlockingIOError, InterruptedError):
|
|
return None
|
|
offset_ns, now_ns = _clock_offset_ns(), time.monotonic_ns()
|
|
stamps = [data for level, kind, data in ancillary if level == socket.SOL_SOCKET and kind == _SO_TIMESTAMPNS]
|
|
if not stamps:
|
|
raise RuntimeError("RFG datagram arrived without a kernel receive timestamp")
|
|
seconds, nanoseconds = _TIMESPEC.unpack(stamps[0])
|
|
received_ns = seconds * 1_000_000_000 + nanoseconds - offset_ns
|
|
previous, self._clock_offset_ns = self._clock_offset_ns, offset_ns
|
|
if previous is None or abs(offset_ns - previous) > _CLOCK_JUMP_NS or received_ns > now_ns:
|
|
# Queued stamps may predate the step: drop them and the sweep in progress.
|
|
with suppress(BlockingIOError):
|
|
while True:
|
|
sock.recv(_READ_CHUNK_BYTES)
|
|
assert self.assembler is not None
|
|
self.assembler.reset()
|
|
return None
|
|
return datagram, received_ns
|
|
|
|
def _publish_sweep(self, sweep: RawSweep) -> None:
|
|
"""Publish a completed sweep to the mailbox(es), waking any waiter.
|
|
|
|
Dirty sweeps (straddling a switch, or incomplete RFG sweeps) are counted but not
|
|
delivered: the collector re-takes that combination on the next sweep. Clean
|
|
tagged sweeps go to their per-combo slot; untagged sweeps (non-switch mode)
|
|
only update the single newest-sweep mailbox that read_sweep() serves.
|
|
"""
|
|
with self._mailbox_cv:
|
|
self._published_count += 1
|
|
if sweep.dirty:
|
|
self.last_dirty_size = sweep.size
|
|
self._mailbox_cv.notify_all()
|
|
return
|
|
self._latest_sweep = sweep
|
|
if sweep.combo is not None:
|
|
self._combo_slots[sweep.combo] = sweep
|
|
self._mailbox_cv.notify_all()
|
|
|
|
def _publish_error(self, exc: Exception) -> None:
|
|
"""Record ``exc`` as the reader fault and wake any waiter."""
|
|
with self._mailbox_cv:
|
|
self._reader_error = exc
|
|
self._mailbox_cv.notify_all()
|