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

some fixes and improvements

Browse Source
This commit is contained in:
Ayzen
2026-05-28 14:33:12 +03:00
parent 83a934f251
commit eacea436a4
29 changed files with 2114 additions and 424 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python_app/hardware_full/kamil_adc_service.py
+235 -203
View File
@@ -1,4 +1,22 @@
"""Service for acquiring sweeps from the external Kamil ADC collector."""
"""Service for acquiring sweeps from the external Kamil ADC collector.
The external `kamil_adc` binary publishes its samples on a PTY/TTY device as a
stream of 8-byte frames:
* **Start marker**: `0x000A 0xFFFF 0xFFFF 0xFFFF` — delimits sweep boundaries.
* **Point frame**: `0x000A step real_i16 imag_i16` — one complex sample per
frame, with `step` running 1, 2, …, N for an N-point sweep.
The hardware emits sweeps continuously, faster than callers tend to invoke
:meth:`KamilAdcService.acquire`. To avoid TTY-buffer overruns and stale data,
a daemon thread drains the device end of the TTY non-stop, parses complete
sweeps as they arrive, and publishes the **latest** one to a one-slot mailbox.
:meth:`acquire` simply waits for the next sweep to appear in that mailbox.
Sweep length is determined by the first sweep observed at runtime and stays
constant for the life of the service; any later mismatch is treated as a
protocol violation rather than something to silently discard.
"""
from __future__ import annotations
@@ -13,6 +31,7 @@ import signal
import stat
import struct
import subprocess
import threading
import time
import numpy as np
@@ -22,233 +41,255 @@ from python_app.models.run_config_model import RadarSweepModel, RunConfigModel
logger = logging.getLogger(__name__)
# Wire-format constants for the Kamil ADC TTY protocol.
KAMIL_ADC_MARKER = 0x000A
KAMIL_ADC_START_STEP = 0xFFFF
KAMIL_ADC_FRAME_BYTES = 8
KAMIL_ADC_MAX_STEP = 0xFFFE
_RAW_FRAME_STRUCT = struct.Struct("<HHHH")
_POINT_FRAME_STRUCT = struct.Struct("<HHhh")
_START_FRAME = _RAW_FRAME_STRUCT.pack(
KAMIL_ADC_MARKER,
KAMIL_ADC_START_STEP,
KAMIL_ADC_START_STEP,
KAMIL_ADC_START_STEP,
_START_FRAME: bytes = struct.pack(
"<HHHH", KAMIL_ADC_MARKER, KAMIL_ADC_START_STEP, KAMIL_ADC_START_STEP, KAMIL_ADC_START_STEP
)
# Point frames carry signed 16-bit real/imag components; start markers reuse
# the same 8-byte slot but with all four words unsigned. Comparing the raw
# bytes against :data:`_START_FRAME` is therefore the correct boundary check.
_POINT_STRUCT = struct.Struct("<HHhh")
# Larger TTY reads keep up with bursty USB CDC-ACM writers without raising the
# syscall rate. 64 KiB matches the typical Linux PTY buffer size.
_READ_CHUNK_BYTES = 65536
# select() poll interval inside the reader thread — short enough to react to
# `close()` requests, long enough that idle CPU stays near zero.
_READ_POLL_INTERVAL_S = 0.1
class KamilAdcFrameParser:
"""Strict parser for Kamil ADC 4-word TTY frames."""
@staticmethod
def is_packet_start(frame: bytes) -> bool:
"""Return whether `frame` is the packet-start marker."""
return frame == _START_FRAME
@staticmethod
def parse_point(frame: bytes, expected_step: int) -> complex:
"""Parse one `0x000A step real imag` frame and validate ordering."""
if len(frame) != KAMIL_ADC_FRAME_BYTES:
raise ValueError(
f"Kamil ADC frame must be {KAMIL_ADC_FRAME_BYTES} bytes, got {len(frame)}"
)
marker, step, real, imag = _POINT_FRAME_STRUCT.unpack(frame)
if marker != KAMIL_ADC_MARKER:
raise ValueError(f"Kamil ADC marker mismatch: got 0x{marker:04x}, expected 0x000a")
if step != expected_step:
raise ValueError(f"Kamil ADC step mismatch: got {step}, expected {expected_step}")
return complex(real, imag)
def _parse_point_frame(frame: bytes, expected_step: int) -> complex:
"""Parse one 8-byte point frame; validate marker and step ordering."""
marker, step, real, imag = _POINT_STRUCT.unpack(frame)
if marker != KAMIL_ADC_MARKER:
raise ValueError(f"Kamil ADC marker mismatch: got 0x{marker:04x}, expected 0x000a")
if step != expected_step:
raise ValueError(f"Kamil ADC step mismatch: got {step}, expected {expected_step}")
return complex(real, imag)
@dataclass(slots=True)
class KamilAdcTtyReader:
"""Read full Kamil ADC sweep packets from a nonblocking TTY stream."""
"""Background-thread TTY reader publishing the latest completed sweep.
The reader spawns a daemon thread on :meth:`open` which continuously
drains the TTY, parses frames into complete sweeps, and stores the most
recent one in a single-slot mailbox. Consumers call :meth:`read_sweep` to
take that sweep; if a newer one arrives before the consumer reads, it
overwrites the previous unread value — by design, since consumers always
want the freshest data.
"""
tty_path: str
_fd: int | None = field(init=False, default=None, repr=False)
_buffer: bytearray = field(init=False, default_factory=bytearray, repr=False)
_packet_start_pending: bool = field(init=False, default=False, 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: np.ndarray | None = field(init=False, default=None, repr=False)
_reader_error: Exception | None = field(init=False, default=None, repr=False)
_locked_points: int | None = field(init=False, default=None, repr=False)
_published_count: int = field(init=False, default=0, repr=False)
def open(self) -> None:
"""Open the configured TTY path for binary reads."""
"""Open the TTY 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)
self._stop_event.clear()
self._latest_sweep = None
self._reader_error = None
self._locked_points = None
self._published_count = 0
self._thread = threading.Thread(
target=self._reader_loop,
name=f"kamil-adc-tty-reader[{self.tty_path}]",
daemon=True,
)
self._thread.start()
def close(self) -> None:
"""Close the TTY file descriptor."""
if self._fd is None:
return
try:
os.close(self._fd)
finally:
self._fd = None
self._buffer.clear()
self._packet_start_pending = False
"""Stop the reader thread and close the TTY descriptor."""
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)
self._thread = None
if self._fd is not None:
try:
os.close(self._fd)
finally:
self._fd = None
self._latest_sweep = None
self._reader_error = None
self._locked_points = None
@property
def locked_points(self) -> int | None:
"""Return the sweep point count established by the first sweep, or `None`."""
return self._locked_points
@property
def published_count(self) -> int:
"""Return the total number of sweeps the reader thread has produced."""
with self._mailbox_cv:
return self._published_count
def read_sweep(
self,
*,
timeout_s: float,
process: subprocess.Popen[bytes] | None = None,
expected_points: int | None = None,
) -> np.ndarray:
"""Read one full packet, optionally discarding packets with an unexpected point count."""
if self._fd is None:
"""Wait for and return the next published sweep.
Raises :class:`TimeoutError` if no sweep arrives within `timeout_s`,
:class:`RuntimeError` if the external collector process exited, and
propagates any exception caught by the reader thread.
"""
if self._thread is None:
raise RuntimeError("Kamil ADC TTY reader is not open")
if expected_points is not None:
if expected_points <= 0:
raise ValueError("Kamil ADC expected points must be > 0")
if expected_points > KAMIL_ADC_MAX_STEP:
raise ValueError(f"Kamil ADC expected points must be <= {KAMIL_ADC_MAX_STEP}")
deadline = time.monotonic() + float(timeout_s)
while True:
values = self._read_one_sweep(deadline, process)
if expected_points is None or int(values.size) == int(expected_points):
return values
logger.warning(
"Discarding Kamil ADC sweep with %d points; expected %d",
int(values.size),
int(expected_points),
)
with self._mailbox_cv:
while True:
# Always deliver a pending sweep first: if the reader thread
# both published a sweep and then died, the consumer should
# still see the good data and only meet the error on the next
# call.
if self._latest_sweep is not None:
sweep = self._latest_sweep
self._latest_sweep = None
return sweep
if self._reader_error is not None:
raise self._reader_error
self._raise_if_process_exited(process)
remaining_s = deadline - time.monotonic()
if remaining_s <= 0.0:
raise TimeoutError(
f"Timed out waiting for Kamil ADC sweep after {float(timeout_s):.3f}s"
)
# Wake periodically so we can re-check process liveness.
self._mailbox_cv.wait(timeout=min(_READ_POLL_INTERVAL_S, remaining_s))
def _read_one_sweep(
self,
deadline: float,
process: subprocess.Popen[bytes] | None,
) -> np.ndarray:
"""Read one packet from start marker to the next start marker."""
if self._packet_start_pending:
self._packet_start_pending = False
else:
self._read_until_packet_start(deadline, process)
# ------------------------------------------------------------------
# Reader-thread internals
# ------------------------------------------------------------------
def _reader_loop(self) -> None:
"""Drain TTY → parse frames → publish completed sweeps until stop."""
buffer = bytearray()
try:
if not self._skip_to_first_start_marker(buffer):
return
while not self._stop_event.is_set():
sweep = self._read_one_sweep(buffer)
if sweep is None:
return
self._publish_sweep(sweep)
except Exception as exc: # noqa: BLE001 — surfaced to the consumer via read_sweep
self._publish_error(exc)
def _skip_to_first_start_marker(self, buffer: bytearray) -> bool:
"""Discard pre-roll bytes until a start marker is consumed from `buffer`."""
while not self._stop_event.is_set():
start_index = buffer.find(_START_FRAME)
if start_index >= 0:
del buffer[: start_index + KAMIL_ADC_FRAME_BYTES]
return True
# Keep just enough trailing bytes that a marker split across read
# boundaries can still be reassembled on the next chunk.
if len(buffer) >= KAMIL_ADC_FRAME_BYTES:
del buffer[: -(KAMIL_ADC_FRAME_BYTES - 1)]
if not self._read_more(buffer):
return False
return False
def _read_one_sweep(self, buffer: bytearray) -> np.ndarray | None:
"""Parse frames from `buffer` until the next start marker; return the sweep."""
values: list[complex] = []
expected_step = 1
while True:
frame = self._read_frame(deadline, process, received_points=len(values))
if KamilAdcFrameParser.is_packet_start(frame):
while not self._stop_event.is_set():
while len(buffer) < KAMIL_ADC_FRAME_BYTES:
if not self._read_more(buffer):
return None
frame = bytes(buffer[:KAMIL_ADC_FRAME_BYTES])
del buffer[:KAMIL_ADC_FRAME_BYTES]
if frame == _START_FRAME:
if not values:
# Two consecutive markers — ignore the empty sweep and keep parsing.
continue
self._packet_start_pending = True
self._validate_and_lock_point_count(len(values))
return np.asarray(values, dtype=np.complex64)
if expected_step > KAMIL_ADC_MAX_STEP:
raise RuntimeError(f"Kamil ADC sweep exceeded {KAMIL_ADC_MAX_STEP} points without packet end")
values.append(KamilAdcFrameParser.parse_point(frame, expected_step))
if self._locked_points is not None and expected_step > self._locked_points:
raise RuntimeError(
f"Kamil ADC sweep exceeded locked point count {self._locked_points} "
"without a start marker"
)
values.append(_parse_point_frame(frame, expected_step))
expected_step += 1
return None
def discard_pending(self, process: subprocess.Popen[bytes] | None = None) -> None:
"""Discard stale bytes while keeping the newest packet-start boundary."""
if self._fd is None:
raise RuntimeError("Kamil ADC TTY reader is not open")
self._buffer.clear()
self._packet_start_pending = False
fd = self._require_fd()
while True:
self._raise_if_process_exited(process)
def _validate_and_lock_point_count(self, points: int) -> None:
"""Lock the point count on the first sweep; reject mismatches thereafter."""
if self._locked_points is None:
self._locked_points = points
logger.info("Kamil ADC sweep point count locked to %d", points)
return
if points != self._locked_points:
raise RuntimeError(
f"Kamil ADC sweep length changed: locked={self._locked_points}, got={points}"
)
def _read_more(self, buffer: bytearray) -> bool:
"""Block on `select` until bytes arrive, then append them to `buffer`.
Returns `False` if the reader was asked to stop, `True` if at least one
byte was appended. Raises on stream-level errors.
"""
fd = self._fd
if fd is None:
return False
while not self._stop_event.is_set():
try:
readable, _, _ = select.select([fd], [], [], 0.0)
readable, _, _ = select.select([fd], [], [], _READ_POLL_INTERVAL_S)
except InterruptedError:
continue
if not readable:
return
continue
try:
chunk = os.read(fd, 4096)
chunk = os.read(fd, _READ_CHUNK_BYTES)
except BlockingIOError:
return
continue
except OSError as exc:
if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}:
return
raise RuntimeError(f"Failed to drain Kamil ADC TTY `{self.tty_path}`: {exc}") from exc
continue
raise RuntimeError(
f"Failed to read Kamil ADC TTY `{self.tty_path}`: {exc}"
) from exc
if not chunk:
raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while draining")
self._buffer.extend(chunk)
self._keep_latest_packet_start_tail()
raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while reading")
buffer.extend(chunk)
return True
return False
def _keep_latest_packet_start_tail(self) -> None:
"""Keep only bytes from the latest complete packet-start marker onward."""
start_index = self._buffer.rfind(_START_FRAME)
if start_index >= 0:
del self._buffer[:start_index]
return
if len(self._buffer) >= KAMIL_ADC_FRAME_BYTES:
del self._buffer[:-KAMIL_ADC_FRAME_BYTES + 1]
def _publish_sweep(self, sweep: np.ndarray) -> None:
"""Store `sweep` as the latest mailbox value, overwriting any prior unread one."""
with self._mailbox_cv:
self._latest_sweep = sweep
self._published_count += 1
self._mailbox_cv.notify()
def _read_until_packet_start(
self,
deadline: float,
process: subprocess.Popen[bytes] | None,
) -> None:
while True:
start_index = self._buffer.find(_START_FRAME)
if start_index >= 0:
del self._buffer[: start_index + KAMIL_ADC_FRAME_BYTES]
return
if len(self._buffer) >= KAMIL_ADC_FRAME_BYTES:
del self._buffer[:-KAMIL_ADC_FRAME_BYTES + 1]
self._read_available(deadline, process)
def _read_frame(
self,
deadline: float,
process: subprocess.Popen[bytes] | None,
*,
received_points: int,
expected_points: int | None = None,
) -> bytes:
while len(self._buffer) < KAMIL_ADC_FRAME_BYTES:
self._read_available(deadline, process, received_points, expected_points)
frame = bytes(self._buffer[:KAMIL_ADC_FRAME_BYTES])
del self._buffer[:KAMIL_ADC_FRAME_BYTES]
return frame
def _read_available(
self,
deadline: float,
process: subprocess.Popen[bytes] | None,
received_points: int | None = None,
expected_points: int | None = None,
) -> None:
self._raise_if_process_exited(process)
remaining_s = deadline - time.monotonic()
if remaining_s <= 0.0:
if received_points is None or expected_points is None:
if received_points is not None:
raise TimeoutError(
f"Timed out waiting for Kamil ADC sweep end: received {received_points} points"
)
raise TimeoutError("Timed out waiting for Kamil ADC packet-start marker")
raise TimeoutError(
f"Timed out waiting for Kamil ADC sweep: received {received_points}/{expected_points} points"
)
fd = self._require_fd()
wait_s = min(0.05, remaining_s)
try:
readable, _, _ = select.select([fd], [], [], wait_s)
except InterruptedError:
return
if not readable:
return
try:
chunk = os.read(fd, 4096)
except BlockingIOError:
return
except OSError as exc:
if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}:
return
raise RuntimeError(f"Failed to read Kamil ADC TTY `{self.tty_path}`: {exc}") from exc
if not chunk:
raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while reading")
self._buffer.extend(chunk)
def _require_fd(self) -> int:
if self._fd is None:
raise RuntimeError("Kamil ADC TTY reader is not open")
return self._fd
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()
@staticmethod
def _raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None:
@@ -261,14 +302,13 @@ class KamilAdcTtyReader:
@dataclass(slots=True)
class KamilAdcService:
"""Launch `kamil_adc` and acquire TTY sweeps."""
"""Launch the external `kamil_adc` collector and serve its sweeps."""
config: RunConfigModel
_process: subprocess.Popen[bytes] | None = field(init=False, default=None, repr=False)
_reader: KamilAdcTtyReader | None = field(init=False, default=None, repr=False)
_settings: RadarSweepModel | None = field(init=False, default=None, repr=False)
_frequency_hz: np.ndarray | None = field(init=False, default=None, repr=False)
_expected_points: int | None = field(init=False, default=None, repr=False)
def __post_init__(self) -> None:
self._validate_config()
@@ -281,10 +321,9 @@ class KamilAdcService:
return [executable_path, *adc.args, f"tty:{adc.tty_path}"]
def open(self) -> None:
"""Launch the collector and open its TTY stream."""
"""Launch the collector and start the TTY reader thread."""
if self._reader is not None:
return
previous_tty_identity = _prepare_tty_path_for_collector(self.config.radar.kamil_adc.tty_path)
try:
self._start_process()
@@ -297,50 +336,40 @@ class KamilAdcService:
raise
def close(self) -> None:
"""Close TTY and stop the external collector."""
"""Stop the TTY reader and the external collector process."""
if self._reader is not None:
with suppress(Exception):
self._reader.close()
self._reader = None
self._stop_process()
def configure(self, sweep: RadarSweepModel) -> None:
"""Store sweep settings used to construct the synthetic frequency axis."""
"""Store sweep settings used to build the synthetic frequency axis."""
self._validate_sweep(sweep)
self._settings = sweep
self._frequency_hz = None
self._expected_points = None
def read_device_limits(self) -> dict[str, float | int]:
"""Kamil ADC has no runtime-readable sweep limit API."""
raise RuntimeError("Kamil ADC device limits are not available")
def acquire(self) -> SweepResult:
"""Acquire one Kamil ADC sweep as S21; fill S11 with explicit zeros."""
"""Return the most recent completed sweep as S21 (S11 filled with zeros)."""
if self._settings is None:
raise RuntimeError("Kamil ADC service is not configured")
if self._reader is None:
raise RuntimeError("Kamil ADC service is not open")
process = self._process
if process is None or process.poll() is not None:
code = None if process is None else process.poll()
raise RuntimeError(f"Kamil ADC process is not running (code={code})")
return_code = None if process is None else process.poll()
raise RuntimeError(f"Kamil ADC process is not running (code={return_code})")
self._reader.discard_pending(process)
s21 = self._reader.read_sweep(
timeout_s=self.config.radar.kamil_adc.sweep_timeout_s,
process=process,
expected_points=self._expected_points,
)
points = int(s21.size)
if points <= 0:
raise RuntimeError("Kamil ADC sweep contained no points")
if self._expected_points is None:
self._expected_points = points
self._frequency_hz = self._build_frequency_axis(points)
logger.info("Kamil ADC sweep point count locked to %d", points)
if self._frequency_hz is None:
if self._frequency_hz is None or self._frequency_hz.size != points:
self._frequency_hz = self._build_frequency_axis(points)
return SweepResult(
x=self._frequency_hz.copy(),
@@ -350,10 +379,13 @@ class KamilAdcService:
},
)
# ------------------------------------------------------------------
# Process / TTY lifecycle
# ------------------------------------------------------------------
def _start_process(self) -> None:
if self._process is not None and self._process.poll() is None:
return
adc = self.config.radar.kamil_adc
env = os.environ.copy()
env.update(adc.env)
@@ -371,11 +403,8 @@ class KamilAdcService:
def _stop_process(self) -> None:
process = self._process
self._process = None
if process is None:
if process is None or process.poll() is not None:
return
if process.poll() is not None:
return
with suppress(ProcessLookupError):
os.killpg(process.pid, signal.SIGTERM)
try:
@@ -383,7 +412,6 @@ class KamilAdcService:
return
except subprocess.TimeoutExpired:
pass
with suppress(ProcessLookupError):
os.killpg(process.pid, signal.SIGKILL)
process.wait(timeout=1.0)
@@ -401,6 +429,10 @@ class KamilAdcService:
f"Timed out waiting for Kamil ADC TTY `{adc.tty_path}` to be created by the collector"
)
# ------------------------------------------------------------------
# Validation helpers
# ------------------------------------------------------------------
def _validate_config(self) -> None:
if not self.config.is_kamil_adc:
raise RuntimeError("KamilAdcService requires radar.model='kamil_adc'")