563 lines
21 KiB
Python
563 lines
21 KiB
Python
"""Service for acquiring sweeps from the external Kamil ADC collector."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from contextlib import suppress
|
|
from dataclasses import dataclass, field
|
|
import errno
|
|
import logging
|
|
import os
|
|
from pathlib import Path
|
|
import select
|
|
import signal
|
|
import stat
|
|
import struct
|
|
import subprocess
|
|
import time
|
|
|
|
import numpy as np
|
|
|
|
from python_app.hardware_full.librevna_driver.models import SweepResult
|
|
from python_app.models.run_config_model import RadarSweepModel, RunConfigModel
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
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,
|
|
)
|
|
|
|
|
|
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)
|
|
|
|
|
|
@dataclass(slots=True)
|
|
class KamilAdcTtyReader:
|
|
"""Read full Kamil ADC sweep packets from a nonblocking TTY stream."""
|
|
|
|
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)
|
|
|
|
def open(self) -> None:
|
|
"""Open the configured TTY path for binary reads."""
|
|
if self._fd is not None:
|
|
return
|
|
self._fd = os.open(self.tty_path, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
|
|
|
|
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
|
|
|
|
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:
|
|
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),
|
|
)
|
|
|
|
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)
|
|
|
|
values: list[complex] = []
|
|
expected_step = 1
|
|
while True:
|
|
frame = self._read_frame(deadline, process, received_points=len(values))
|
|
if KamilAdcFrameParser.is_packet_start(frame):
|
|
if not values:
|
|
continue
|
|
self._packet_start_pending = True
|
|
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))
|
|
expected_step += 1
|
|
|
|
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)
|
|
try:
|
|
readable, _, _ = select.select([fd], [], [], 0.0)
|
|
except InterruptedError:
|
|
continue
|
|
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 drain 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()
|
|
|
|
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 _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
|
|
|
|
@staticmethod
|
|
def _raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None:
|
|
if process is None:
|
|
return
|
|
return_code = process.poll()
|
|
if return_code is not None:
|
|
raise RuntimeError(f"Kamil ADC process exited with code {return_code}")
|
|
|
|
|
|
@dataclass(slots=True)
|
|
class KamilAdcService:
|
|
"""Launch `kamil_adc` and acquire TTY 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()
|
|
|
|
@property
|
|
def command(self) -> list[str]:
|
|
"""Return external collector command including the generated TTY argument."""
|
|
adc = self.config.radar.kamil_adc
|
|
executable_path = str(Path(adc.executable_path).expanduser())
|
|
return [executable_path, *adc.args, f"tty:{adc.tty_path}"]
|
|
|
|
def open(self) -> None:
|
|
"""Launch the collector and open its TTY stream."""
|
|
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()
|
|
self._wait_for_tty(previous_tty_identity)
|
|
reader = KamilAdcTtyReader(self.config.radar.kamil_adc.tty_path)
|
|
reader.open()
|
|
self._reader = reader
|
|
except Exception:
|
|
self.close()
|
|
raise
|
|
|
|
def close(self) -> None:
|
|
"""Close TTY and stop the external collector."""
|
|
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."""
|
|
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."""
|
|
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})")
|
|
|
|
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:
|
|
self._frequency_hz = self._build_frequency_axis(points)
|
|
return SweepResult(
|
|
x=self._frequency_hz.copy(),
|
|
traces={
|
|
"s11": np.zeros(points, dtype=np.complex64),
|
|
"s21": s21,
|
|
},
|
|
)
|
|
|
|
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)
|
|
logger.info("Starting Kamil ADC collector: %s", " ".join(self.command))
|
|
self._process = subprocess.Popen(
|
|
self.command,
|
|
cwd=str(Path(adc.project_dir).expanduser()),
|
|
env=env,
|
|
stdin=subprocess.DEVNULL,
|
|
stdout=subprocess.DEVNULL,
|
|
stderr=subprocess.STDOUT,
|
|
start_new_session=True,
|
|
)
|
|
|
|
def _stop_process(self) -> None:
|
|
process = self._process
|
|
self._process = None
|
|
if process is None:
|
|
return
|
|
if process.poll() is not None:
|
|
return
|
|
|
|
with suppress(ProcessLookupError):
|
|
os.killpg(process.pid, signal.SIGTERM)
|
|
try:
|
|
process.wait(timeout=self.config.radar.kamil_adc.stop_timeout_s)
|
|
return
|
|
except subprocess.TimeoutExpired:
|
|
pass
|
|
|
|
with suppress(ProcessLookupError):
|
|
os.killpg(process.pid, signal.SIGKILL)
|
|
process.wait(timeout=1.0)
|
|
|
|
def _wait_for_tty(self, previous_identity: tuple[object, ...] | None) -> None:
|
|
adc = self.config.radar.kamil_adc
|
|
deadline = time.monotonic() + adc.startup_timeout_s
|
|
while time.monotonic() < deadline:
|
|
KamilAdcTtyReader._raise_if_process_exited(self._process)
|
|
identity = _tty_identity(adc.tty_path)
|
|
if identity is not None and identity != previous_identity:
|
|
return
|
|
time.sleep(0.05)
|
|
raise TimeoutError(
|
|
f"Timed out waiting for Kamil ADC TTY `{adc.tty_path}` to be created by the collector"
|
|
)
|
|
|
|
def _validate_config(self) -> None:
|
|
if not self.config.is_kamil_adc:
|
|
raise RuntimeError("KamilAdcService requires radar.model='kamil_adc'")
|
|
if self.config.radar.driver_mode != "native":
|
|
raise RuntimeError("Kamil ADC requires radar.driver_mode='native'")
|
|
|
|
adc = self.config.radar.kamil_adc
|
|
if not adc.project_dir:
|
|
raise ValueError("radar.kamil_adc.project_dir is required")
|
|
if not adc.executable_path:
|
|
raise ValueError("radar.kamil_adc.executable_path is required")
|
|
if not adc.tty_path:
|
|
raise ValueError("radar.kamil_adc.tty_path is required")
|
|
if any(arg.startswith("tty:") for arg in adc.args):
|
|
raise ValueError("radar.kamil_adc.args must not contain tty:<path>; use tty_path instead")
|
|
if adc.startup_timeout_s <= 0.0:
|
|
raise ValueError("radar.kamil_adc.startup_timeout_s must be > 0")
|
|
if adc.sweep_timeout_s <= 0.0:
|
|
raise ValueError("radar.kamil_adc.sweep_timeout_s must be > 0")
|
|
if adc.stop_timeout_s <= 0.0:
|
|
raise ValueError("radar.kamil_adc.stop_timeout_s must be > 0")
|
|
|
|
project_dir = Path(adc.project_dir).expanduser()
|
|
if not project_dir.is_dir():
|
|
raise RuntimeError(f"radar.kamil_adc.project_dir is not a directory: {project_dir}")
|
|
executable_path = Path(adc.executable_path).expanduser()
|
|
if not executable_path.is_file():
|
|
raise RuntimeError(f"radar.kamil_adc.executable_path is not a file: {executable_path}")
|
|
if not os.access(executable_path, os.X_OK):
|
|
raise RuntimeError(f"radar.kamil_adc.executable_path is not executable: {executable_path}")
|
|
|
|
@staticmethod
|
|
def _validate_sweep(sweep: RadarSweepModel) -> None:
|
|
if float(sweep.stop_hz) < float(sweep.start_hz):
|
|
raise ValueError("Kamil ADC sweep stop_hz must be >= start_hz")
|
|
|
|
def _build_frequency_axis(self, points: int) -> np.ndarray:
|
|
if self._settings is None:
|
|
raise RuntimeError("Kamil ADC service is not configured")
|
|
return np.linspace(
|
|
float(self._settings.start_hz),
|
|
float(self._settings.stop_hz),
|
|
int(points),
|
|
dtype=np.float32,
|
|
)
|
|
|
|
|
|
def _tty_identity(path: str) -> tuple[object, ...] | None:
|
|
try:
|
|
if os.path.islink(path):
|
|
stat_result = os.lstat(path)
|
|
return (
|
|
"link",
|
|
os.readlink(path),
|
|
int(stat_result.st_dev),
|
|
int(stat_result.st_ino),
|
|
int(stat_result.st_mtime_ns),
|
|
)
|
|
stat_result = os.stat(path)
|
|
except FileNotFoundError:
|
|
return None
|
|
return (
|
|
"node",
|
|
int(stat_result.st_dev),
|
|
int(stat_result.st_ino),
|
|
int(stat_result.st_mtime_ns),
|
|
)
|
|
|
|
|
|
def _prepare_tty_path_for_collector(path: str) -> tuple[object, ...] | None:
|
|
"""Remove stale generated TTY links before starting the external collector."""
|
|
try:
|
|
stat_result = os.lstat(path)
|
|
except FileNotFoundError:
|
|
return None
|
|
|
|
if stat.S_ISLNK(stat_result.st_mode) or stat.S_ISREG(stat_result.st_mode):
|
|
os.unlink(path)
|
|
return None
|
|
return None
|
|
|
|
|
|
def apply_kamil_adc_laser_control(config: RunConfigModel) -> bool:
|
|
"""Apply Kamil ADC laser settings exactly through the legacy device_main command sequence."""
|
|
laser = config.radar.laser_control
|
|
if not laser.enabled:
|
|
return False
|
|
|
|
_validate_laser_control_config(config)
|
|
|
|
from python_app.hardware_full.laser_control.controller import DEVICE_MAIN_MESSAGE_ID, LaserController
|
|
from python_app.hardware_full.laser_control.models import VariationType
|
|
|
|
controller = LaserController(
|
|
port=laser.port,
|
|
pi_coeff1_p=laser.pi_coeff1_p,
|
|
pi_coeff1_i=laser.pi_coeff1_i,
|
|
pi_coeff2_p=laser.pi_coeff2_p,
|
|
pi_coeff2_i=laser.pi_coeff2_i,
|
|
)
|
|
try:
|
|
controller.connect()
|
|
controller.reset()
|
|
mode = laser.mode.strip().lower()
|
|
if mode == "manual":
|
|
manual = laser.manual
|
|
controller.set_manual_mode(
|
|
temp1=manual.temp1,
|
|
temp2=manual.temp2,
|
|
current1=manual.current1,
|
|
current2=manual.current2,
|
|
message_id=DEVICE_MAIN_MESSAGE_ID,
|
|
)
|
|
return True
|
|
if mode == "variation":
|
|
variation = laser.variation
|
|
try:
|
|
variation_type = VariationType[variation.variation_type]
|
|
except KeyError as exc:
|
|
raise ValueError(
|
|
f"Unsupported radar.laser_control.variation.variation_type: {variation.variation_type}"
|
|
) from exc
|
|
|
|
controller.set_manual_mode(
|
|
temp1=variation.static_temp1,
|
|
temp2=variation.static_temp2,
|
|
current1=variation.static_current1,
|
|
current2=variation.static_current2,
|
|
message_id=DEVICE_MAIN_MESSAGE_ID,
|
|
)
|
|
controller.start_variation(
|
|
variation_type=variation_type,
|
|
params={
|
|
"static_temp1": variation.static_temp1,
|
|
"static_temp2": variation.static_temp2,
|
|
"static_current1": variation.static_current1,
|
|
"static_current2": variation.static_current2,
|
|
"min_value": variation.min_value,
|
|
"max_value": variation.max_value,
|
|
"step": variation.step,
|
|
"time_step": variation.time_step,
|
|
"delay_time": variation.delay_time,
|
|
},
|
|
)
|
|
return True
|
|
raise RuntimeError(f"Unsupported laser_control mode: {laser.mode}")
|
|
finally:
|
|
controller.disconnect()
|
|
|
|
|
|
def _validate_laser_control_config(config: RunConfigModel) -> None:
|
|
laser = config.radar.laser_control
|
|
if not laser.port:
|
|
raise ValueError("radar.laser_control.port is required when laser_control is enabled")
|
|
mode = laser.mode.strip().lower()
|
|
if mode not in {"manual", "variation"}:
|
|
raise ValueError("radar.laser_control.mode must be 'manual' or 'variation'")
|
|
if mode == "variation" and not laser.variation.variation_type:
|
|
raise ValueError("radar.laser_control.variation.variation_type is required")
|