awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add RFG UDP source to rph_radar

Browse Source
This commit is contained in:
Ayzen
2026-10-06 19:36:00 +03:00
parent a185e61495
commit cfe81f782b
29 changed files with 817 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python_app/hardware_full/rph_radar/protocol.py
+78 -6
View File
@@ -1,10 +1,15 @@
"""Whole-sweep RPH1 packets from the collector's PTY (little endian).
"""RPH sweep sources, both 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.
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
@@ -18,6 +23,8 @@ 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)
@@ -87,3 +94,68 @@ class RphRadarStreamParser:
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)),
)]
python_app/hardware_full/rph_radar/service.py
+30 -16
View File
@@ -1,8 +1,9 @@
"""Service that launches the RadioPhotonic radar (RPH) collector and serves processed sweeps.
"""Service that serves processed RadioPhotonic radar (RPH) sweeps.
Owns the lifecycle of the external collector process and its TTY reader, and maps
each raw (main, reference) sweep to an :class:`SweepResult` on the fixed
processing grid via :class:`RphRadarSweepProcessor`.
With ``source == "e502"`` it owns the external collector process and its TTY; with
``"rfg_udp"`` it reads the RFG board's UDP stream directly. Each raw (main,
reference) sweep maps to a :class:`SweepResult` on the fixed processing grid via
:class:`RphRadarSweepProcessor`.
"""
from __future__ import annotations
@@ -22,15 +23,13 @@ import time
import numpy as np
from python_app.hardware_full.rph_radar.processing import RphRadarSweepProcessor
from python_app.hardware_full.rph_radar.protocol import RawSweep
from python_app.hardware_full.rph_radar.tty_reader import (
RphRadarTtyReader,
raise_if_process_exited,
)
from python_app.hardware_full.laser_control.conversions import variation_step_count
from python_app.hardware_full.rph_radar.protocol import RawSweep, RfgSweepAssembler
from python_app.hardware_full.rph_radar.stream_reader import RphRadarStreamReader, raise_if_process_exited
from python_app.hardware_full.librevna_driver.models import SweepResult
from python_app.models.dataset_model import ComboKey
from python_app.models.run_config_schema import RadarSweepModel, RunConfigModel
from python_app.models.run_config_validation import validate_rph_radar_model
from python_app.models.run_config_validation import validate_rfg_source, validate_rph_radar_model
logger = logging.getLogger(__name__)
@@ -51,12 +50,12 @@ _STOP_KILL_GRACE_S = 0.5
@dataclass(slots=True)
class RphRadarService:
"""Launch the external RadioPhotonic radar (RPH) collector and serve its processed sweeps."""
"""Serve processed RadioPhotonic radar (RPH) sweeps from the E-502 collector or the RFG stream."""
config: RunConfigModel
collector_driven: bool = False
_process: subprocess.Popen[bytes] | None = field(init=False, default=None, repr=False)
_reader: RphRadarTtyReader | None = field(init=False, default=None, repr=False)
_reader: RphRadarStreamReader | None = field(init=False, default=None, repr=False)
_processor: RphRadarSweepProcessor | None = field(init=False, default=None, repr=False)
_rejected_count: int = field(init=False, default=0, repr=False)
_prepared_combo: ComboKey | None = field(init=False, default=None, repr=False)
@@ -81,13 +80,25 @@ class RphRadarService:
return cmd
def open(self, *, stop_event: threading.Event | None = None) -> None:
"""Launch the collector and start the TTY reader thread.
"""Bind the RFG UDP stream, or launch the E-502 collector and start its TTY reader.
An optional ``stop_event`` lets a caller abort the TTY-wait loop promptly
(e.g. on shutdown) instead of blocking for the full startup timeout.
"""
if self._reader is not None:
return
adc = self.config.radar.rph_radar
if adc.source == "rfg_udp":
variation = self.config.radar.laser_control.variation
steps = variation_step_count(variation.min_value, variation.max_value, variation.step)
step_ns = variation.time_step * 1000
# Lower bound of the sweep period: the firmware idles at least delay_time - 1 ms.
period_ns = steps * step_ns + (variation.delay_time - 1) * 1_000_000
reader = RphRadarStreamReader(adc.udp_bind, RfgSweepAssembler(steps, step_ns, period_ns))
reader.open()
self._reader = reader
logger.info("RadioPhotonic radar (RPH) RFG stream opened on %s (%d steps per sweep)", adc.udp_bind, steps)
return
if self._process is not None and self._process.poll() is None:
raise RuntimeError("Previous RPH collector did not stop; cannot start a fresh capture")
self._stop_event = stop_event
@@ -95,7 +106,7 @@ class RphRadarService:
try:
self._start_process()
self._wait_for_tty(previous_tty_identity, stop_event=stop_event)
reader = RphRadarTtyReader(self.config.radar.rph_radar.tty_path)
reader = RphRadarStreamReader(self.config.radar.rph_radar.tty_path)
reader.open()
self._reader = reader
logger.info("RadioPhotonic radar (RPH) service opened")
@@ -105,7 +116,7 @@ class RphRadarService:
raise
def close(self) -> None:
"""Stop the TTY reader and the external collector process."""
"""Stop the reader and, for E-502, the external collector process."""
logger.debug("Closing RadioPhotonic radar (RPH) service")
self._prepared_combo = None
if self._reader is not None:
@@ -157,7 +168,7 @@ class RphRadarService:
if self._reader is None:
raise RuntimeError("RadioPhotonic radar (RPH) service is not open")
process = self._process
if process is None or process.poll() is not None:
if self.config.radar.rph_radar.source == "e502" and (process is None or process.poll() is not None):
return_code = None if process is None else process.poll()
raise RuntimeError(f"RadioPhotonic radar (RPH) collector is not running (code={return_code})")
@@ -334,6 +345,9 @@ class RphRadarService:
adc = self.config.radar.rph_radar
validate_rph_radar_model(adc)
if adc.source == "rfg_udp":
validate_rfg_source(self.config)
return
if not adc.executable_path:
raise ValueError("radar.rph_radar.executable_path is required")
if not adc.tty_path:
python_app/hardware_full/rph_radar/tty_reader.py → python_app/hardware_full/rph_radar/stream_reader.py
+106 -34
View File
@@ -1,8 +1,9 @@
"""Background TTY reader publishing the latest completed RadioPhotonic radar (RPH) sweep.
"""Background reader publishing the latest completed RadioPhotonic radar (RPH) sweep.
The collector emits sweeps continuously, faster than callers invoke
:meth:`RphRadarService.acquire`. A daemon thread drains the device end of the
TTY non-stop, feeds the bytes to a :class:`RphRadarStreamParser`, and stores the
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
@@ -12,16 +13,20 @@ 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 RphRadarStreamParser, RawSweep
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__)
@@ -31,6 +36,13 @@ _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:
@@ -42,11 +54,18 @@ def raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None:
raise RuntimeError(f"RadioPhotonic radar (RPH) collector exited with code {return_code}")
@dataclass(slots=True)
class RphRadarTtyReader:
"""Daemon-thread TTY reader publishing the latest completed sweep."""
def _clock_offset_ns() -> int:
return time.clock_gettime_ns(time.CLOCK_REALTIME) - time.monotonic_ns()
tty_path: str
@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)
@@ -58,12 +77,29 @@ class RphRadarTtyReader:
_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 and start the background reader thread."""
"""Open the TTY or bind the UDP socket and start the background reader thread."""
if self._fd is not None:
return
self._fd = os.open(self.tty_path, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
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 = {}
@@ -71,21 +107,20 @@ class RphRadarTtyReader:
self._published_count = 0
self._thread = threading.Thread(
target=self._reader_loop,
name=f"rph-radar-tty-reader[{self.tty_path}]",
name=f"rph-radar-reader[{self.endpoint}]",
daemon=True,
)
try:
self._thread.start()
except Exception:
self._thread = None
fd, self._fd = self._fd, None
os.close(fd)
self._close_descriptor()
raise
logger.info("RadioPhotonic radar (RPH) TTY reader started on %s", self.tty_path)
logger.info("RadioPhotonic radar (RPH) reader started on %s", self.endpoint)
def close(self) -> None:
"""Stop the reader thread and close the TTY descriptor."""
logger.debug("Stopping RadioPhotonic radar (RPH) TTY reader on %s", self.tty_path)
"""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()
@@ -94,15 +129,18 @@ class RphRadarTtyReader:
if self._thread.is_alive():
logger.warning("RadioPhotonic radar (RPH) reader thread did not stop within 1.0s")
self._thread = None
if self._fd is not None:
try:
os.close(self._fd)
finally:
self._fd = 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."""
@@ -123,12 +161,12 @@ class RphRadarTtyReader:
error caught by the reader thread.
"""
if self._thread is None:
raise RuntimeError("RadioPhotonic radar (RPH) TTY reader is not open")
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) TTY reader is closed")
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.
@@ -142,23 +180,27 @@ class RphRadarTtyReader:
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"
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 TTY, parse sweeps, and publish each completed one until stop."""
"""Drain the descriptor, parse sweeps, and publish each completed one until stop."""
parser = RphRadarStreamParser()
try:
while not self._stop_event.is_set():
chunk = self._read_available()
if not chunk:
continue
for sweep in parser.feed(chunk):
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.tty_path)
logger.exception("RadioPhotonic radar (RPH) reader thread failed on %s", self.endpoint)
self._publish_error(exc)
def _read_available(self) -> bytes:
@@ -183,15 +225,44 @@ class RphRadarTtyReader:
except OSError as exc:
if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}:
return b""
raise RuntimeError(f"Failed to read RadioPhotonic radar (RPH) TTY `{self.tty_path}`: {exc}") from exc
raise RuntimeError(f"Failed to read RadioPhotonic radar (RPH) source `{self.endpoint}`: {exc}") from exc
if not chunk:
raise RuntimeError(f"RadioPhotonic radar (RPH) TTY `{self.tty_path}` closed while reading")
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 (those that straddled a switch transition) are counted but not
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.
@@ -199,6 +270,7 @@ class RphRadarTtyReader:
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