some fixes and improvements
This commit is contained in:
Ayzen
@@ -0,0 +1,956 @@
|
||||
"""
|
||||
MIMO GPR — локализация через пересечение эллипсов
|
||||
==================================================
|
||||
|
||||
Физика в двух словах:
|
||||
Пик A-скана пары (Tx_i, Rx_j) на задержке τ означает:
|
||||
|Tx → объект| + |объект → Rx| = v · τ
|
||||
Это уравнение эллипса. Истинный отражатель лежит на
|
||||
пересечении всех Tx/Rx-эллипсов (по одному на измеренную пару).
|
||||
|
||||
Алгоритм:
|
||||
1. S(f) → IFFT → A-сканы всех Tx/Rx-пар
|
||||
2. Поиск пиков: SNR = пик / медиана > порог
|
||||
3. Для каждого пика → мягкий эллипс в аккумуляторе
|
||||
(с компенсацией геометрического и углового затухания)
|
||||
4. CLEAN: найти максимум → убрать его эллипсы → повторить
|
||||
|
||||
О параметре SHELL_SIGMA:
|
||||
Аккумулятор — это «мягкое голосование». Каждый эллипс добавляет
|
||||
не единицу, а гауссово-взвешенный вклад:
|
||||
w = exp(−δ²/2σ²), где δ = |R_Tx + R_Rx − v·τ|
|
||||
SHELL_SIGMA — ширина этой гауссовой оболочки.
|
||||
Слишком широко → ghost-цели не подавляются.
|
||||
Слишком узко → вклад падает до нуля из-за дискретности сетки.
|
||||
Оптимум: ~ 0.4 × δZ, где δZ = v/(2B) — разрешение по глубине.
|
||||
|
||||
О score:
|
||||
score = количество пар, чей эллипс проходит
|
||||
через данную точку с невязкой δ < 3σ.
|
||||
Принимает целые значения от 0 до N_pairs.
|
||||
Максимальный score у истинного объекта = N_pairs (все пары согласны).
|
||||
Ghost-цели имеют меньший score, т.к. согласуются только
|
||||
с частью пар.
|
||||
|
||||
О компенсации затухания:
|
||||
При генерации S(f) сигнал ослаблен:
|
||||
geo(i,j) = 1/(R_Tx · R_Rx) — геометрическое ослабление
|
||||
pat(i,j) = cos²(θ_Tx)·cos²(θ_Rx) — диаграмма направленности
|
||||
Без компенсации глубокий/угловой отражатель будет недооценён.
|
||||
Компенсация: делим вес каждого пика на ожидаемое затухание
|
||||
в точке z_apparent, вычисленное для данной пары антенн.
|
||||
|
||||
Геометрия: карта строится в плоскости XZ при y=ELLIPS_PLANE_Y,
|
||||
но бистатические дальности считаются до 3D-координат антенн.
|
||||
"""
|
||||
|
||||
"""
|
||||
MIMO GPR — локализация через пересечение эллипсов
|
||||
==================================================
|
||||
Версия для реальных данных
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.lines import Line2D
|
||||
from scipy.signal import find_peaks
|
||||
from scipy.ndimage import gaussian_filter, label
|
||||
from pathlib import Path
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Tuple
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 0.1 ИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ
|
||||
# ══════════════════════════════════════════════════════
|
||||
INPUT_IDX = [0, 1, 2, 3]
|
||||
OUTPUT_IDX = [0, 1]
|
||||
|
||||
# Частотный диапазон и глубинный gate.
|
||||
F_START = 40 * 1e8
|
||||
F_STOP = 60 * 1e8
|
||||
MIN_DEPTH = 3.0
|
||||
MAX_DEPTH = 14.5
|
||||
|
||||
# Движение радара между Tx-событиями.
|
||||
# Новая схема измерения: один Tx излучает, все 4 Rx принимают одновременно;
|
||||
# затем излучает следующий Tx. Поэтому motion correction имеет 2 временные точки,
|
||||
# а не 8 последовательных точек для отдельных Tx/Rx-пар.
|
||||
SPEED_M_S = 1.11
|
||||
LOOK_ANGLE_DEG = 12.0 # угол между направлением движения и осью дальности Z
|
||||
TX_SWEEP_TIME_S = 0.072 # время одного sweep для одного Tx-события
|
||||
TX_SWITCH_TIME_S = 1e-5
|
||||
APPLY_FREQ_PHASE_CORRECTION = True # True: корректировать движение внутри sweep до IFFT
|
||||
|
||||
# Данные и вычитание среднего фона.
|
||||
# True: вычитать среднее по всем снимкам в папке, убирая прямую волну и статику.
|
||||
# False: использовать данные как есть.
|
||||
BG_SUBTRACT = True
|
||||
BG_PATH = Path('/Users/ivan_root/Downloads/Telegram_dwnld/moving_26052026/20260526_10-6000_751_50k_triplet_fast_1/preprocessed')
|
||||
DATA_PATH = Path('/Users/ivan_root/Downloads/Telegram_dwnld/moving_26052026/20260526_10-6000_751_50k_triplet_fast_1/preprocessed/0046_id47_ns3315537839930')
|
||||
|
||||
# Параметры поиска пиков и CLEAN по карте эллипсов.
|
||||
MODE = 'point' # 'point' или 'extended'
|
||||
SNR_THRESH = 3.8 # минимальный SNR пика A-скана
|
||||
SNR_COMP_MAX = 25.0 # верхний предел компенсированного SNR
|
||||
COMP_POWER = 0.22 # степень компенсации геометрического/углового затухания
|
||||
MAX_OBJECTS = 15
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 0.2 КОНФИГИ АНТЕНН И ДВИЖЕНИЯ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
# Физические координаты антенн по их реальным индексам, [м].
|
||||
# Формат: x, (x, y) или (x, y, z). X - поперечная ось, Z - наклонная
|
||||
# дальность вдоль оси радара, Y - привязанная к радару вертикальная ось.
|
||||
# Карта строится в плоскости y=ELLIPS_PLANE_Y, но расстояния Tx/Rx считаются
|
||||
# в полном 3D. Это даёт 2D проекцию бистатических эллипсов без 3D volume search.
|
||||
ELLIPS_PLANE_Y = 0.0
|
||||
|
||||
TX_POSITIONS = {
|
||||
0: (-75.0 * 0.01, 0.0, 0.0),
|
||||
1: ( 75.0 * 0.01, 0.0, 0.0),
|
||||
}
|
||||
|
||||
RX_POSITIONS = {
|
||||
0: ( 19.0 * 0.01, 0.0, 0.0),
|
||||
1: ( 45.0 * 0.01, 0.0, 0.0),
|
||||
2: (-45.0 * 0.01, 0.0, 0.0),
|
||||
3: (-19.0 * 0.01, 0.0, 0.0),
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class MotionConfig:
|
||||
"""
|
||||
Конфигурация движения для кадра с параллельным приёмом по Rx.
|
||||
|
||||
speed_m_s:
|
||||
Линейная скорость радара во время съёмки кадра.
|
||||
|
||||
look_angle_deg:
|
||||
Угол между направлением движения и осью дальности Z.
|
||||
Если движение почти вдоль дальности, ставьте угол близкий к 0°.
|
||||
|
||||
tx_sweep_time_s:
|
||||
Время sweep для одного Tx-события. Все Rx этого Tx имеют один timestamp.
|
||||
|
||||
tx_switch_time_s:
|
||||
Время переключения между соседними Tx-событиями.
|
||||
|
||||
pair_order_phys:
|
||||
Порядок пар в физических индексах, как они записаны в данных:
|
||||
[(tx_phys_1, rx_phys_1), (tx_phys_2, rx_phys_2), ...]
|
||||
При расчёте движения пары с одинаковым tx_phys считаются одновременными.
|
||||
|
||||
reference_mode:
|
||||
Относительно какого момента считаем dt:
|
||||
- 'frame_center' : середина между первым и последним Tx-событием;
|
||||
- 'first_tx_event' : центр первого Tx-события.
|
||||
|
||||
direction_sign:
|
||||
Знак движения по оси дальности.
|
||||
+1 -> более поздние Tx-события выглядят глубже;
|
||||
-1 -> более поздние Tx-события выглядят ближе.
|
||||
|
||||
apply_freq_phase_correction:
|
||||
True -> до IFFT компенсировать движение внутри одного Tx-sweep,
|
||||
потому что частоты измеряются последовательно снизу вверх.
|
||||
"""
|
||||
speed_m_s: float = 0.0
|
||||
look_angle_deg: float = 0.0
|
||||
tx_sweep_time_s: float = 0.15
|
||||
tx_switch_time_s: float = 1e-5
|
||||
pair_order_phys: List[Tuple[int, int]] = field(default_factory=lambda: [
|
||||
(tx_phys, rx_phys)
|
||||
for tx_phys in sorted(OUTPUT_IDX)
|
||||
for rx_phys in sorted(INPUT_IDX)
|
||||
])
|
||||
reference_mode: str = 'frame_center'
|
||||
direction_sign: float = +1.0
|
||||
apply_freq_phase_correction: bool = True
|
||||
|
||||
|
||||
MOTION_CONFIG = MotionConfig(
|
||||
speed_m_s=SPEED_M_S,
|
||||
look_angle_deg=LOOK_ANGLE_DEG,
|
||||
tx_sweep_time_s=TX_SWEEP_TIME_S,
|
||||
tx_switch_time_s=TX_SWITCH_TIME_S,
|
||||
pair_order_phys=[
|
||||
(0, 0), (0, 1), (0, 2), (0, 3),
|
||||
(1, 0), (1, 1), (1, 2), (1, 3),
|
||||
],
|
||||
reference_mode='frame_center',
|
||||
direction_sign=+1.0,
|
||||
apply_freq_phase_correction=APPLY_FREQ_PHASE_CORRECTION,
|
||||
)
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 0.3 НЕИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ (ЛУЧШЕ НЕ ТРОГАТЬ)
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
eps_r = 1.0
|
||||
v = 3e8 / np.sqrt(eps_r)
|
||||
|
||||
|
||||
def positions_to_xyz(position_dict):
|
||||
coords = []
|
||||
for idx in sorted(position_dict):
|
||||
pos = np.asarray(position_dict[idx], dtype=float)
|
||||
if pos.ndim == 0:
|
||||
pos = np.array([float(pos), 0.0, 0.0], dtype=float)
|
||||
elif pos.shape == (2,):
|
||||
pos = np.array([pos[0], pos[1], 0.0], dtype=float)
|
||||
elif pos.shape != (3,):
|
||||
raise ValueError(f'Позиция антенны {idx} должна быть x, (x,y) или (x,y,z), получено {pos}')
|
||||
coords.append(pos)
|
||||
return np.vstack(coords)
|
||||
|
||||
|
||||
tx_xyz = positions_to_xyz(TX_POSITIONS)
|
||||
rx_xyz = positions_to_xyz(RX_POSITIONS)
|
||||
x_tx, y_tx, z_tx = tx_xyz.T
|
||||
x_rx, y_rx, z_rx = rx_xyz.T
|
||||
|
||||
# Границы сетки аккумулятора в плоскости y=ELLIPS_PLANE_Y.
|
||||
x_ant = np.concatenate([x_tx, x_rx])
|
||||
x_min, x_max = x_ant.min() - 2.0, x_ant.max() + 2.0
|
||||
z_min, z_max = 0.2, MAX_DEPTH
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 1. ЗАГРУЗКА РЕАЛЬНЫХ ДАННЫХ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def load_mimo_data(data_path, input_idx, output_idx):
|
||||
data_path = Path(data_path)
|
||||
s21_data = {}
|
||||
freq_data = {}
|
||||
|
||||
for f in data_path.glob("i*_o*_s21.npy"):
|
||||
name = f.stem
|
||||
parts = name.split('_')
|
||||
i_tx_phys = int(parts[1][1:])
|
||||
i_rx_phys = int(parts[0][1:])
|
||||
|
||||
if i_tx_phys not in output_idx or i_rx_phys not in input_idx:
|
||||
continue
|
||||
|
||||
# Переводим физический индекс → порядковый (0,1,2,...)
|
||||
i_tx = sorted(output_idx).index(i_tx_phys)
|
||||
i_rx = sorted(input_idx).index(i_rx_phys)
|
||||
|
||||
s21_data[(i_tx, i_rx)] = np.load(f)
|
||||
freq_file = data_path / f"i{i_rx_phys}_o{i_tx_phys}_freq.npy"
|
||||
freq_data[(i_tx, i_rx)] = np.load(freq_file)
|
||||
|
||||
tx_indices = sorted(set(k[0] for k in s21_data))
|
||||
rx_indices = sorted(set(k[1] for k in s21_data))
|
||||
n_tx = len(tx_indices)
|
||||
n_rx = len(rx_indices)
|
||||
|
||||
print(f"Загружено пар: {len(s21_data)}")
|
||||
print(f"Передатчиков: {n_tx}, Приёмников: {n_rx}")
|
||||
|
||||
return s21_data, freq_data, n_tx, n_rx
|
||||
|
||||
|
||||
# Загрузка данных
|
||||
s21_data, freq_data, N_tx, N_rx = load_mimo_data(DATA_PATH, INPUT_IDX, OUTPUT_IDX)
|
||||
N_pairs = len(s21_data)
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 1б. ВЫЧИСЛЕНИЕ СРЕДНЕГО ФОНА ПО ВСЕМ СНИМКАМ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def compute_background(bg_path, input_idx, output_idx):
|
||||
"""
|
||||
Для каждой пары (i_tx, i_rx) усредняем S21 по всем снимкам в папке.
|
||||
|
||||
Возвращает:
|
||||
bg : dict[(i_tx, i_rx)] → np.array (complex), усреднённый S21
|
||||
"""
|
||||
bg_path = Path(bg_path)
|
||||
snapshots = sorted(bg_path.glob("*/")) # каждый подкаталог — один снимок
|
||||
snapshots = [s for s in snapshots if s.is_dir()]
|
||||
|
||||
if len(snapshots) == 0:
|
||||
print("⚠️ Снимков для фона не найдено, BG_SUBTRACT отключён.")
|
||||
return None
|
||||
|
||||
print(f"Вычисление фона по {len(snapshots)} снимкам...", end=" ", flush=True)
|
||||
|
||||
# Накопитель: для каждой пары суммируем S21
|
||||
bg_sum = {}
|
||||
bg_count = {}
|
||||
|
||||
for snap_dir in snapshots:
|
||||
for f in snap_dir.glob("i*_o*_s21.npy"):
|
||||
name = f.stem
|
||||
parts = name.split('_')
|
||||
i_tx_phys = int(parts[1][1:])
|
||||
i_rx_phys = int(parts[0][1:])
|
||||
|
||||
if i_tx_phys not in output_idx or i_rx_phys not in input_idx:
|
||||
continue
|
||||
|
||||
i_tx = sorted(output_idx).index(i_tx_phys)
|
||||
i_rx = sorted(input_idx).index(i_rx_phys)
|
||||
key = (i_tx, i_rx)
|
||||
|
||||
s21 = np.load(f)
|
||||
if key not in bg_sum:
|
||||
bg_sum[key] = np.zeros_like(s21, dtype=complex)
|
||||
bg_count[key] = 0
|
||||
bg_sum[key] += s21
|
||||
bg_count[key] += 1
|
||||
|
||||
bg = {key: bg_sum[key] / bg_count[key] for key in bg_sum}
|
||||
print(f"готово. Пар: {len(bg)}, снимков на пару: "
|
||||
f"{list(bg_count.values())[0] if bg_count else 0}")
|
||||
return bg
|
||||
|
||||
|
||||
if BG_SUBTRACT:
|
||||
background = compute_background(BG_PATH, INPUT_IDX, OUTPUT_IDX)
|
||||
if background is None:
|
||||
BG_SUBTRACT = False # автоматически выключаем если нет данных
|
||||
else:
|
||||
background = None
|
||||
print("BG_SUBTRACT = False, вычитание фона отключено.")
|
||||
|
||||
|
||||
# Проверка частот (берём из первой пары как референс)
|
||||
first_key = list(freq_data.keys())[0]
|
||||
freqs = freq_data[first_key]
|
||||
|
||||
|
||||
|
||||
# Проверим что частоты одинаковые для всех пар
|
||||
for key, freq in freq_data.items():
|
||||
if not np.allclose(freq, freqs):
|
||||
print(f"⚠️ Частоты для пары {key} отличаются!")
|
||||
|
||||
|
||||
mask_freq = (freqs >= F_START) & (freqs <= F_STOP)
|
||||
|
||||
freqs = freqs[mask_freq]
|
||||
|
||||
f_min, f_max = freqs[0], freqs[-1]
|
||||
BW = f_max - f_min
|
||||
N_f = len(freqs)
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 2. ПАРАМЕТРЫ СИСТЕМЫ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
# Проверка соответствия координатов антенн
|
||||
assert len(x_tx) == N_tx, f"x_tx должен содержать {N_tx} элементов"
|
||||
assert len(x_rx) == N_rx, f"x_rx должен содержать {N_rx} элементов"
|
||||
|
||||
|
||||
# SHELL_SIGMA — ширина гауссовой оболочки
|
||||
SHELL_SIGMA = v / BW * 0.5 # [м]
|
||||
|
||||
|
||||
# Сетка аккумулятора
|
||||
|
||||
x_grid = np.linspace(x_min, x_max, 300)
|
||||
z_grid = np.linspace(z_min, z_max, 300)
|
||||
XX, ZZ = np.meshgrid(x_grid, z_grid)
|
||||
|
||||
def range_to_tx(i_tx, X, Z, yy=ELLIPS_PLANE_Y):
|
||||
return np.sqrt((X - x_tx[i_tx])**2 + (yy - y_tx[i_tx])**2 + (Z - z_tx[i_tx])**2)
|
||||
|
||||
|
||||
def range_to_rx(i_rx, X, Z, yy=ELLIPS_PLANE_Y):
|
||||
return np.sqrt((X - x_rx[i_rx])**2 + (yy - y_rx[i_rx])**2 + (Z - z_rx[i_rx])**2)
|
||||
|
||||
|
||||
def bistatic_ranges(i_tx, i_rx, X, Z, yy=ELLIPS_PLANE_Y):
|
||||
Rtx = range_to_tx(i_tx, X, Z, yy=yy)
|
||||
Rrx = range_to_rx(i_rx, X, Z, yy=yy)
|
||||
return Rtx, Rrx
|
||||
|
||||
|
||||
def antenna_boresight_cos_z(R, z_ant, Z):
|
||||
# В локальных координатах радара все антенны смотрят вдоль +Z.
|
||||
return (Z - z_ant) / (R + 1e-12)
|
||||
|
||||
|
||||
# Расстояния от 2D-сетки до 3D-координат антенн.
|
||||
# Это не 3D поиск: мы строим карту только в плоскости y=ELLIPS_PLANE_Y,
|
||||
# но каждая точка карты получает корректную бистатическую дальность в 3D.
|
||||
R_tx_grid = {i: range_to_tx(i, XX, ZZ) for i in range(N_tx)}
|
||||
R_rx_grid = {j: range_to_rx(j, XX, ZZ) for j in range(N_rx)}
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 3. ВЫЧИСЛЕНИЕ A-СКАНОВ ИЗ РЕАЛЬНЫХ S21
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def compute_ascan(s21, freq, f_start, f_stop, window=True):
|
||||
"""
|
||||
S21(f) → IFFT → A-скан с правильным частотным сдвигом.
|
||||
|
||||
Проблема наивного подхода (buf[:n] = s21):
|
||||
IFFT считает, что спектр начинается с 0 Гц.
|
||||
Реальные данные начинаются с f[0] > 0, поэтому
|
||||
нулевая задержка смещается и в A-скане появляются биения.
|
||||
|
||||
Правильный подход — сдвиг спектра:
|
||||
Шаг частотной сетки df вычисляется из данных.
|
||||
Индекс первой частоты: k0 = round(f[0] / df).
|
||||
Данные кладутся в H[k0 : k0+n], а не в H[0 : n].
|
||||
Тогда IFFT корректно восстанавливает временной сигнал
|
||||
с нулевой задержкой в t=0.
|
||||
|
||||
Размер FFT:
|
||||
Минимум для покрытия всего диапазона [0, f[-1]]:
|
||||
min_len = 2 * (k0 + n - 1)
|
||||
Округляем вверх до степени двойки для скорости FFT.
|
||||
"""
|
||||
mask_freq_ = (freq >= f_start) & (freq <= f_stop)
|
||||
freq = freq[mask_freq_]
|
||||
s21 = s21[mask_freq_]
|
||||
|
||||
n = len(freq)
|
||||
if n < 2:
|
||||
raise ValueError("Слишком мало частотных точек")
|
||||
|
||||
# Шаг частотной сетки
|
||||
df = (freq[-1] - freq[0]) / (n - 1)
|
||||
if df <= 0:
|
||||
raise ValueError("Частоты не возрастают")
|
||||
|
||||
# Индекс первой частоты в полной сетке от 0 до f[-1]
|
||||
k0 = int(np.round(freq[0] / df))
|
||||
|
||||
# Минимальный размер FFT, округлённый до степени двойки
|
||||
min_len = 2 * (k0 + n - 1)
|
||||
n_fft = 1 << int(np.ceil(np.log2(min_len)))
|
||||
|
||||
|
||||
# Временна́я ось — пересчитываем из нового n_fft
|
||||
dt = 1.0 / (n_fft * df)
|
||||
t_sec = np.arange(n_fft, dtype=float) * dt
|
||||
|
||||
# Оконная функция (подавление боковых лепестков IFFT)
|
||||
s = s21 * np.hanning(n) if window else s21.copy()
|
||||
|
||||
# Спектр со сдвигом: данные на своём месте в частотной сетке
|
||||
H = np.zeros(n_fft, dtype=np.complex128)
|
||||
H[k0 : k0 + n] = s
|
||||
|
||||
y = np.abs(np.fft.ifft(H))
|
||||
|
||||
|
||||
return t_sec[:y.size], y[:y.size]
|
||||
|
||||
|
||||
def _build_phys_to_logical_maps(output_idx, input_idx):
|
||||
tx_map = {phys: k for k, phys in enumerate(sorted(output_idx))}
|
||||
rx_map = {phys: k for k, phys in enumerate(sorted(input_idx))}
|
||||
return tx_map, rx_map
|
||||
|
||||
|
||||
def _tx_event_order_from_pairs(pair_order_phys):
|
||||
"""Возвращает порядок Tx-событий по первому появлению tx_phys в pair_order_phys."""
|
||||
tx_events = []
|
||||
for tx_phys, _ in pair_order_phys:
|
||||
if tx_phys not in tx_events:
|
||||
tx_events.append(tx_phys)
|
||||
return tx_events
|
||||
|
||||
|
||||
def compute_pair_timestamps(config: MotionConfig,
|
||||
output_idx=OUTPUT_IDX,
|
||||
input_idx=INPUT_IDX) -> Tuple[Dict[Tuple[int, int], Dict], List[Dict]]:
|
||||
"""
|
||||
Для каждой пары возвращает t_start/t_center Tx-события и грубый pair shift.
|
||||
|
||||
Пары с одинаковым tx_phys получают одинаковое время, потому что Rx принимают
|
||||
параллельно. t_start/t_center также используются для частотной phase correction
|
||||
внутри sweep до IFFT.
|
||||
"""
|
||||
tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx)
|
||||
|
||||
expected_pairs = {(tx, rx) for tx in output_idx for rx in input_idx}
|
||||
observed_pairs = set(config.pair_order_phys)
|
||||
missing_pairs = expected_pairs - observed_pairs
|
||||
extra_pairs = observed_pairs - expected_pairs
|
||||
if missing_pairs:
|
||||
raise ValueError(f"pair_order_phys не содержит пары: {sorted(missing_pairs)}")
|
||||
if extra_pairs:
|
||||
raise ValueError(f"pair_order_phys содержит лишние пары: {sorted(extra_pairs)}")
|
||||
if len(config.pair_order_phys) != len(observed_pairs):
|
||||
raise ValueError("pair_order_phys содержит повторяющиеся пары")
|
||||
|
||||
tx_event_order = _tx_event_order_from_pairs(config.pair_order_phys)
|
||||
tx_event_timing = {}
|
||||
for event_idx, tx_phys in enumerate(tx_event_order):
|
||||
if tx_phys not in tx_phys_to_log:
|
||||
raise ValueError(f"Tx {tx_phys} отсутствует в OUTPUT_IDX={output_idx}")
|
||||
|
||||
t_start = event_idx * (config.tx_sweep_time_s + config.tx_switch_time_s)
|
||||
t_center = t_start + 0.5 * config.tx_sweep_time_s
|
||||
t_stop = t_start + config.tx_sweep_time_s
|
||||
tx_event_timing[tx_phys] = {
|
||||
'tx_event_idx': event_idx,
|
||||
't_start_s': t_start,
|
||||
't_center_s': t_center,
|
||||
't_stop_s': t_stop,
|
||||
}
|
||||
|
||||
if not tx_event_order:
|
||||
return {}, []
|
||||
|
||||
first_center = tx_event_timing[tx_event_order[0]]['t_center_s']
|
||||
last_center = tx_event_timing[tx_event_order[-1]]['t_center_s']
|
||||
if config.reference_mode == 'frame_center':
|
||||
t_ref = 0.5 * (first_center + last_center)
|
||||
elif config.reference_mode == 'first_tx_event':
|
||||
t_ref = first_center
|
||||
else:
|
||||
raise ValueError("reference_mode must be 'frame_center' or 'first_tx_event'")
|
||||
|
||||
cos_theta = np.cos(np.radians(config.look_angle_deg))
|
||||
pair_timestamps: Dict[Tuple[int, int], Dict] = {}
|
||||
rows: List[Dict] = []
|
||||
|
||||
for order_idx, (tx_phys, rx_phys) in enumerate(config.pair_order_phys):
|
||||
if rx_phys not in rx_phys_to_log:
|
||||
raise ValueError(f"Rx {rx_phys} отсутствует в INPUT_IDX={input_idx}")
|
||||
|
||||
i_tx = tx_phys_to_log[tx_phys]
|
||||
i_rx = rx_phys_to_log[rx_phys]
|
||||
event = tx_event_timing[tx_phys]
|
||||
dt_ref = event['t_center_s'] - t_ref
|
||||
dz_motion = config.direction_sign * config.speed_m_s * dt_ref * cos_theta
|
||||
dtau_motion = 2.0 * dz_motion / v
|
||||
|
||||
row = {
|
||||
'order_idx': order_idx,
|
||||
'tx_event_idx': event['tx_event_idx'],
|
||||
'tx_phys': tx_phys,
|
||||
'rx_phys': rx_phys,
|
||||
'i_tx': i_tx,
|
||||
'i_rx': i_rx,
|
||||
't_start_s': event['t_start_s'],
|
||||
't_center_s': event['t_center_s'],
|
||||
't_stop_s': event['t_stop_s'],
|
||||
'dt_ref_s': dt_ref,
|
||||
'dz_motion_m': dz_motion,
|
||||
'dtau_motion_s': dtau_motion,
|
||||
}
|
||||
rows.append(row)
|
||||
pair_timestamps[(i_tx, i_rx)] = row.copy()
|
||||
|
||||
return pair_timestamps, rows
|
||||
|
||||
|
||||
def compute_frequency_sample_times(freq_full: np.ndarray,
|
||||
f_start: float,
|
||||
f_stop: float,
|
||||
pair_info: Dict,
|
||||
sweep_time_s: float) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""Возвращает индексы выбранных частот и абсолютное время каждой точки sweep."""
|
||||
mask = (freq_full >= f_start) & (freq_full <= f_stop)
|
||||
idx = np.flatnonzero(mask)
|
||||
if idx.size == 0:
|
||||
raise ValueError('После частотной обрезки не осталось точек для phase correction')
|
||||
|
||||
if len(freq_full) < 2:
|
||||
t_abs = np.full(idx.size, pair_info['t_center_s'], dtype=float)
|
||||
else:
|
||||
dt_freq = sweep_time_s / (len(freq_full) - 1)
|
||||
t_abs = pair_info['t_start_s'] + idx * dt_freq
|
||||
|
||||
return idx, t_abs
|
||||
|
||||
|
||||
def apply_intra_sweep_phase_correction(s21: np.ndarray,
|
||||
freq_full: np.ndarray,
|
||||
pair_info: Dict,
|
||||
config: MotionConfig,
|
||||
wave_speed: float,
|
||||
f_start: float,
|
||||
f_stop: float):
|
||||
"""
|
||||
Частотная motion correction до IFFT.
|
||||
|
||||
Частоты внутри одного Tx-sweep измеряются последовательно снизу вверх.
|
||||
Каждая частотная точка имеет своё положение радара, поэтому приводим её
|
||||
фазу к центру этого Tx-события:
|
||||
phi(f) = 2*pi*f * 2*dz_intra / wave_speed
|
||||
dz_intra = v_motion*cos(theta)*(t_freq - t_center_tx_event)
|
||||
После этого A-скан строится уже из phase-corrected S21.
|
||||
"""
|
||||
s21_corr = np.array(s21, dtype=np.complex128, copy=True)
|
||||
idx, t_abs = compute_frequency_sample_times(
|
||||
freq_full=freq_full,
|
||||
f_start=f_start,
|
||||
f_stop=f_stop,
|
||||
pair_info=pair_info,
|
||||
sweep_time_s=config.tx_sweep_time_s,
|
||||
)
|
||||
|
||||
dt_intra = t_abs - pair_info['t_center_s']
|
||||
theta = np.radians(config.look_angle_deg)
|
||||
delta_range = config.direction_sign * config.speed_m_s * np.cos(theta) * dt_intra
|
||||
delta_path = 2.0 * delta_range
|
||||
phi = 2.0 * np.pi * freq_full[idx] * delta_path / wave_speed
|
||||
|
||||
if config.apply_freq_phase_correction:
|
||||
s21_corr[idx] *= np.exp(1j * phi)
|
||||
|
||||
meta = {
|
||||
'enabled': bool(config.apply_freq_phase_correction),
|
||||
'freq_idx': idx,
|
||||
't_abs_s': t_abs,
|
||||
'dt_intra_s': dt_intra,
|
||||
'delta_range_m': delta_range,
|
||||
'delta_path_m': delta_path,
|
||||
'phi_rad': phi,
|
||||
}
|
||||
return s21_corr, meta
|
||||
|
||||
|
||||
pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG)
|
||||
|
||||
print("Вычисление A-сканов из реальных данных...", end=" ", flush=True)
|
||||
|
||||
A_RAW = {} # A_RAW[(i,j)] — A-скан без внутрисвиповой phase correction
|
||||
A = {} # A[(i,j)] — phase-corrected A-скан, используемый дальше
|
||||
T_h = {} # T_h[(i,j)] — временна́я ось для этой пары [с]
|
||||
Z_h = {} # Z_h[(i,j)] — ось глубины [м]
|
||||
phase_meta = {}
|
||||
|
||||
for (i, j), s21 in s21_data.items():
|
||||
s21_proc = np.array(s21, dtype=np.complex128, copy=True)
|
||||
|
||||
# Вычитание фона в частотной области
|
||||
if BG_SUBTRACT and background is not None and (i, j) in background:
|
||||
s21_proc = s21_proc - background[(i, j)]
|
||||
# Примечание: вычитаем до обрезки по частоте и до окна —
|
||||
# фон вычисляется из полных (необрезанных) данных,
|
||||
# поэтому вычитание корректно в полном частотном диапазоне.
|
||||
|
||||
if (i, j) not in pair_timestamps:
|
||||
raise KeyError(f'Нет временной информации для пары {(i, j)}')
|
||||
|
||||
t_pair_raw, a_pair_raw = compute_ascan(s21_proc, freq_data[(i, j)],
|
||||
f_start=F_START, f_stop=F_STOP)
|
||||
s21_corr, meta = apply_intra_sweep_phase_correction(
|
||||
s21=s21_proc,
|
||||
freq_full=freq_data[(i, j)],
|
||||
pair_info=pair_timestamps[(i, j)],
|
||||
config=MOTION_CONFIG,
|
||||
wave_speed=v,
|
||||
f_start=F_START,
|
||||
f_stop=F_STOP,
|
||||
)
|
||||
t_pair, a_pair = compute_ascan(s21_corr, freq_data[(i, j)],
|
||||
f_start=F_START, f_stop=F_STOP)
|
||||
|
||||
T_h[(i, j)] = t_pair
|
||||
Z_h[(i, j)] = t_pair * v / 2
|
||||
A_RAW[(i, j)] = a_pair_raw
|
||||
A[(i, j)] = a_pair
|
||||
phase_meta[(i, j)] = meta
|
||||
|
||||
bg_label = "с вычитанием фона" if BG_SUBTRACT else "без вычитания фона"
|
||||
phase_label = "с внутрисвиповой phase correction" if MOTION_CONFIG.apply_freq_phase_correction else "без внутрисвиповой phase correction"
|
||||
print(f"готово ({bg_label}, {phase_label}).")
|
||||
|
||||
|
||||
# Общая ось z для визуализации и поиска пиков
|
||||
# (берём максимальный диапазон по всем парам)
|
||||
z_h = Z_h[list(Z_h.keys())[0]] # все пары дают одинаковую ось, если freq совпадают
|
||||
t_h = T_h[list(T_h.keys())[0]]
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 4. ДЕТЕКТИРОВАНИЕ ПИКОВ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def attenuation_at_depth(i_tx, i_rx, z_app):
|
||||
"""
|
||||
Ожидаемое ослабление geo·pattern для точки прямо под виртуальным
|
||||
центром пары на глубине z_app.
|
||||
|
||||
Используется для компенсации: реальный SNR пика делится на это
|
||||
значение, чтобы вес глубокого/углового объекта не занижался.
|
||||
"""
|
||||
xc = (x_tx[i_tx] + x_rx[i_rx]) / 2.0 # виртуальный центр в плоскости карты
|
||||
Rtx, Rrx = bistatic_ranges(i_tx, i_rx, xc, z_app)
|
||||
geo = 1.0 / (Rtx * Rrx + 1e-12)
|
||||
cos_tx = antenna_boresight_cos_z(Rtx, z_tx[i_tx], z_app)
|
||||
cos_rx = antenna_boresight_cos_z(Rrx, z_rx[i_rx], z_app)
|
||||
pat = cos_tx**2 * cos_rx**2
|
||||
return geo * pat + 1e-30 # +ε чтобы не делить на ноль
|
||||
|
||||
|
||||
def find_peaks_snr(i_tx, i_rx, SNR_COMP_MAX = SNR_COMP_MAX):
|
||||
"""
|
||||
Поиск пиков A-скана.
|
||||
Возвращает список dict:
|
||||
z_app — кажущаяся глубина [м]
|
||||
tau — задержка [с]
|
||||
snr_raw — SNR без компенсации = пик / медиана
|
||||
snr_comp— SNR с компенсацией ослабления (используется в аккумуляторе)
|
||||
"""
|
||||
ascan = A[(i_tx, i_rx)]
|
||||
z_h_ij = Z_h[(i_tx, i_rx)]
|
||||
t_h_ij = T_h[(i_tx, i_rx)]
|
||||
i_min = np.searchsorted(z_h_ij, MIN_DEPTH)
|
||||
i_max = np.searchsorted(z_h_ij, MAX_DEPTH)
|
||||
|
||||
noise = np.median(ascan[i_min:i_max]) # Добавил чтобы было удобно считать SNR отнсительно выбранной области
|
||||
min_dist = max(4, int(v / (2*BW) / (z_h_ij[1] - z_h_ij[0]) * 0.7))
|
||||
idx, _ = find_peaks(ascan[i_min:i_max],
|
||||
height=noise * SNR_THRESH,
|
||||
distance=min_dist)
|
||||
idx += i_min
|
||||
result = []
|
||||
for p in idx:
|
||||
z_app = float(z_h_ij[p])
|
||||
snr_raw = float(ascan[p] / noise)
|
||||
atten = attenuation_at_depth(i_tx, i_rx, z_app)
|
||||
atten_norm = atten / attenuation_at_depth(i_tx, i_rx, 3.0) # Референсная глубина - 3м
|
||||
snr_comp = snr_raw / (atten_norm ** COMP_POWER + 1e-12)
|
||||
snr_comp = min(snr_comp, SNR_COMP_MAX) # ← clipping
|
||||
result.append({'z_app': z_app,
|
||||
'tau': float(t_h_ij[p]),
|
||||
'snr_raw': snr_raw,
|
||||
'snr_comp': snr_comp})
|
||||
return result
|
||||
|
||||
|
||||
peaks = {(i, j): find_peaks_snr(i, j)
|
||||
for i in range(N_tx) for j in range(N_rx)}
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 6. ПОИСК ОБЪЕКТОВ
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
def find_centroid(acc_s, iz, ix, rpz, rpx):
|
||||
"""
|
||||
Взвешенный центроид аккумулятора в окрестности (iz, ix).
|
||||
"""
|
||||
NZ, NX = acc_s.shape
|
||||
iz0 = max(0, iz - rpz); iz1 = min(NZ, iz + rpz)
|
||||
ix0 = max(0, ix - rpx); ix1 = min(NX, ix + rpx)
|
||||
patch = acc_s[iz0:iz1, ix0:ix1].copy()
|
||||
W = patch.sum()
|
||||
if W <= 0:
|
||||
return x_grid[ix], z_grid[iz]
|
||||
rows = np.arange(iz0, iz1)[:, None] * np.ones(patch.shape)
|
||||
cols = np.ones(patch.shape) * np.arange(ix0, ix1)[None, :]
|
||||
iz_c = int(round(np.clip((rows * patch).sum() / W, 0, NZ-1)))
|
||||
ix_c = int(round(np.clip((cols * patch).sum() / W, 0, NX-1)))
|
||||
return x_grid[ix_c], z_grid[iz_c]
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 8. MOTION-AWARE FIRST-ORDER CORRECTION
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
"""
|
||||
Первый блок для движения без изменения продакшн-пайплайна выше.
|
||||
|
||||
Новая схема измерения:
|
||||
1. Tx2 излучает, Rx0..Rx3 принимают одновременно.
|
||||
2. После переключения Tx3 излучает, Rx0..Rx3 снова принимают одновременно.
|
||||
3. Поэтому timestamp задаётся не для 8 отдельных пар, а для 2 Tx-событий.
|
||||
4. Для уже найденных пиков формируем motion-corrected версию:
|
||||
tau_corr, z_corr
|
||||
|
||||
Это first-order модель: считаем, что весь sweep одного Tx-события имеет один
|
||||
центр времени. Если смещение внутри sweep станет заметным, следующим шагом
|
||||
нужна per-frequency коррекция до IFFT.
|
||||
"""
|
||||
|
||||
|
||||
# compute_pair_timestamps(...) уже определён выше, потому что phase correction нужна до IFFT.
|
||||
|
||||
|
||||
def build_corrected_peaks(peaks_in: Dict[Tuple[int, int], List[Dict]],
|
||||
pair_timestamps: Dict[Tuple[int, int], Dict]) -> Dict[Tuple[int, int], List[Dict]]:
|
||||
"""
|
||||
Формирует словарь corrected_peaks с motion-aware поправками.
|
||||
|
||||
Для каждого пика добавляет:
|
||||
tau_phase, z_phase — пик после внутрисвиповой phase correction;
|
||||
tau_corr, z_corr — тот же пик после грубого сдвига Tx-события;
|
||||
dz_motion, dtau_motion.
|
||||
|
||||
Для совместимости также оставляем aliases tau_raw/z_app_raw, но здесь
|
||||
raw означает 'до грубого pair shift', а не до phase correction.
|
||||
"""
|
||||
corrected = {}
|
||||
|
||||
for key, peak_list in peaks_in.items():
|
||||
if key not in pair_timestamps:
|
||||
raise KeyError(f"Нет временной информации для пары {key}")
|
||||
|
||||
info = pair_timestamps[key]
|
||||
dz_motion = info['dz_motion_m']
|
||||
dtau_motion = info['dtau_motion_s']
|
||||
|
||||
corrected_list = []
|
||||
for pk in peak_list:
|
||||
tau_phase = float(pk['tau'])
|
||||
z_phase = float(pk['z_app'])
|
||||
tau_corr = tau_phase + dtau_motion
|
||||
z_corr = 0.5 * v * tau_corr
|
||||
|
||||
pk_corr = dict(pk)
|
||||
pk_corr.update({
|
||||
'tau_phase': tau_phase,
|
||||
'z_phase': z_phase,
|
||||
'tau_raw': tau_phase, # alias для старых диагностических блоков
|
||||
'z_app_raw': z_phase, # alias: пик до грубого pair shift
|
||||
'tau_corr': tau_corr,
|
||||
'z_corr': z_corr,
|
||||
'dz_motion': dz_motion,
|
||||
'dtau_motion': dtau_motion,
|
||||
})
|
||||
corrected_list.append(pk_corr)
|
||||
|
||||
corrected[key] = corrected_list
|
||||
|
||||
return corrected
|
||||
|
||||
|
||||
|
||||
pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG)
|
||||
corrected_peaks = build_corrected_peaks(peaks, pair_timestamps)
|
||||
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════
|
||||
# 9. MOTION-AWARE IMAGE BUILD FROM CORRECTED PEAKS
|
||||
# ══════════════════════════════════════════════════════
|
||||
|
||||
"""
|
||||
Эта ячейка строит motion-aware картинку, используя corrected_peaks из блока выше.
|
||||
|
||||
Что меняется относительно статического продакшн-пайплайна:
|
||||
- в аккумуляторе используется tau_corr вместо tau
|
||||
- в apparent-depth логике CLEAN используется z_corr вместо z_app
|
||||
- score считается по corrected пикам
|
||||
|
||||
Исходные A-сканы остаются теми же, но на графике ниже можно показывать уже
|
||||
motion-corrected положения пиков.
|
||||
"""
|
||||
|
||||
|
||||
def _peak_in_work_depth(pk):
|
||||
return MIN_DEPTH <= pk['z_corr'] <= MAX_DEPTH
|
||||
|
||||
|
||||
|
||||
def build_accumulator_motion(corrected_peaks_in, exclude_z_ranges):
|
||||
acc = np.zeros_like(XX)
|
||||
for i in range(N_tx):
|
||||
for j in range(N_rx):
|
||||
for pk in corrected_peaks_in[(i, j)]:
|
||||
if not _peak_in_work_depth(pk):
|
||||
continue
|
||||
if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges):
|
||||
continue
|
||||
r_total = v * pk['tau_corr']
|
||||
residual = R_tx_grid[i] + R_rx_grid[j] - r_total
|
||||
shell = np.exp(-0.5 * (residual / SHELL_SIGMA)**2)
|
||||
acc += shell * pk['snr_comp']
|
||||
return acc
|
||||
|
||||
|
||||
|
||||
def count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, exclude_z_ranges):
|
||||
count = 0
|
||||
for i in range(N_tx):
|
||||
for j in range(N_rx):
|
||||
for pk in corrected_peaks_in[(i, j)]:
|
||||
if not _peak_in_work_depth(pk):
|
||||
continue
|
||||
if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges):
|
||||
continue
|
||||
Rt, Rr = bistatic_ranges(i, j, x_est, z_est)
|
||||
if abs(Rt + Rr - v * pk['tau_corr']) < SHELL_SIGMA * 6:
|
||||
count += 1
|
||||
break
|
||||
return count
|
||||
|
||||
|
||||
|
||||
def clean_find_motion(corrected_peaks_in, n_search=10, suppress_r_cm=7, thresh_frac=0.05):
|
||||
dx = x_grid[1] - x_grid[0]
|
||||
dz = z_grid[1] - z_grid[0]
|
||||
rpx = int(suppress_r_cm / 100 / dx)
|
||||
rpz = int(suppress_r_cm / 100 / dz)
|
||||
|
||||
excl_z = []
|
||||
found_motion = []
|
||||
acc_initial = build_accumulator_motion(corrected_peaks_in, [])
|
||||
|
||||
for step in range(n_search):
|
||||
acc = build_accumulator_motion(corrected_peaks_in, excl_z)
|
||||
acc_s = gaussian_filter(acc, sigma=3)
|
||||
|
||||
if acc_s.max() < thresh_frac * acc_initial.max():
|
||||
break
|
||||
|
||||
iz, ix = np.unravel_index(acc_s.argmax(), acc_s.shape)
|
||||
x_est, z_est = find_centroid(acc_s, iz, ix, rpz, rpx)
|
||||
score = count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, excl_z)
|
||||
found_motion.append({'x': x_est, 'z': z_est, 'score': score})
|
||||
|
||||
matched = [pk['z_corr']
|
||||
for i in range(N_tx) for j in range(N_rx)
|
||||
for pk in corrected_peaks_in[(i, j)]
|
||||
if _peak_in_work_depth(pk)
|
||||
and not any(lo <= pk['z_corr'] <= hi for lo, hi in excl_z)
|
||||
and abs(sum(bistatic_ranges(i, j, x_est, z_est)) -
|
||||
v * pk['tau_corr']) < SHELL_SIGMA * 3]
|
||||
|
||||
if matched:
|
||||
margin = SHELL_SIGMA * 1.0
|
||||
excl_z.append((min(matched) - margin, max(matched) + margin))
|
||||
|
||||
return found_motion, acc_initial
|
||||
|
||||
|
||||
if MODE == 'point':
|
||||
found_motion, accum_motion = clean_find_motion(corrected_peaks, n_search=MAX_OBJECTS)
|
||||
|
||||
|
||||
# ─── График 2: motion-aware карта накопления ───────────────────────
|
||||
fig, ax = plt.subplots(figsize=(12, 7))
|
||||
acc_motion_s = gaussian_filter(accum_motion, sigma=3)
|
||||
im = ax.imshow(
|
||||
acc_motion_s,
|
||||
extent= [x_grid[0]*100, x_grid[-1]*100, z_grid[-1]*100, z_grid[0]*100],
|
||||
aspect='auto', origin='upper', cmap='hot',
|
||||
vmin=acc_motion_s.max()*0.45, vmax=acc_motion_s.max()*0.95,
|
||||
)
|
||||
plt.colorbar(im, ax=ax, label='Накопленный вес (motion-aware)')
|
||||
ax.plot(x_tx*100, z_tx*100, 'r^', ms=10, label='Tx (XZ projection)', zorder=5)
|
||||
ax.plot(x_rx*100, z_rx*100, 'bv', ms=10, label='Rx (XZ projection)', zorder=5)
|
||||
for obj in found_motion:
|
||||
lbl = f"score={obj['score']}/{N_pairs}"
|
||||
ax.plot(obj['x']*100, obj['z']*100, 'wD', ms=9, zorder=11, markeredgecolor='black', mew=1.2)
|
||||
ax.annotate(lbl, (obj['x']*100, obj['z']*100), textcoords='offset points', xytext=(6, 4),
|
||||
fontsize=8, color='white', bbox=dict(boxstyle='round,pad=0.2', fc='black', alpha=0.5))
|
||||
ax.plot([], [], 'wD', ms=9, markeredgecolor='k', mew=1.2, label='Найденные объекты')
|
||||
ax.set_xlabel('X [см]')
|
||||
ax.set_ylabel('Глубина Z [см]')
|
||||
ax.set_title('Motion-aware карта накопления эллипсов')
|
||||
ax.set_xlim(x_grid[0]*100, x_grid[-1]*100)
|
||||
ax.set_ylim(z_grid[-1]*100, z_grid[0]*100)
|
||||
ax.legend(loc='lower right', fontsize=9)
|
||||
ax.grid(alpha=0.25)
|
||||
ax.invert_yaxis()
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
@@ -18,6 +18,14 @@ enum class LegacyGprMode {
|
||||
Extended,
|
||||
};
|
||||
|
||||
enum class LegacyGprReferenceMode {
|
||||
// t_ref = midpoint between the first and last event centers.
|
||||
FrameCenter,
|
||||
// t_ref = center of the first event. Useful when motion offsets should be
|
||||
// accumulated from frame start, e.g. for tagging frames by their head time.
|
||||
FirstTxEvent,
|
||||
};
|
||||
|
||||
struct ProcessingLiveConfig {
|
||||
std::string processor_mode = "pass_through";
|
||||
std::string pass_through_channel = "s21";
|
||||
@@ -42,6 +50,15 @@ struct ProcessingLiveConfig {
|
||||
std::string gpr_score_mode = "combined";
|
||||
float gpr_speed_m_s = 0.0F;
|
||||
float gpr_look_angle_deg = 0.0F;
|
||||
// Motion model parameters for the legacy GPR pipeline. The direction sign
|
||||
// selects which way later Tx-events appear deeper (+1) or shallower (-1).
|
||||
// Intra-sweep phase correction compensates the motion that happens *inside*
|
||||
// one Tx-sweep before the IFFT — it is independent of the per-pair coarse
|
||||
// tau shift and can be disabled without affecting the rest of the pipeline.
|
||||
// The reference mode picks the anchor used to compute dt_ref per event.
|
||||
float gpr_direction_sign = 1.0F;
|
||||
bool gpr_apply_freq_phase_correction = true;
|
||||
LegacyGprReferenceMode gpr_reference_mode = LegacyGprReferenceMode::FrameCenter;
|
||||
float gpr_snr_thresh = 4.5F;
|
||||
float gpr_snr_comp_max = 25.0F;
|
||||
float gpr_start_freq_mhz = 3000.0F;
|
||||
|
||||
@@ -41,6 +41,18 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error(field_name + " must be one of: point, extended");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_legacy_gpr_reference_mode(
|
||||
const std::string& value, const std::string& field_name
|
||||
) -> LegacyGprReferenceMode {
|
||||
if (value == "frame_center") {
|
||||
return LegacyGprReferenceMode::FrameCenter;
|
||||
}
|
||||
if (value == "first_tx_event") {
|
||||
return LegacyGprReferenceMode::FirstTxEvent;
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: frame_center, first_tx_event");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_score_mode(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "peak" || value == "combined") {
|
||||
return value;
|
||||
@@ -266,6 +278,25 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
}
|
||||
config.gpr_look_angle_deg = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_direction_sign"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_direction_sign must be number");
|
||||
}
|
||||
config.gpr_direction_sign = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_apply_freq_phase_correction"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.gpr_apply_freq_phase_correction must be bool");
|
||||
}
|
||||
config.gpr_apply_freq_phase_correction = found->get<bool>();
|
||||
}
|
||||
if (const auto found = root.find("gpr_reference_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_reference_mode must be string");
|
||||
}
|
||||
config.gpr_reference_mode =
|
||||
parse_legacy_gpr_reference_mode(found->get<std::string>(), "processing.gpr_reference_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_snr_thresh"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_snr_thresh must be number");
|
||||
|
||||
@@ -1,3 +1,37 @@
|
||||
// Legacy MIMO GPR — ellipse-intersection localizer with motion compensation.
|
||||
//
|
||||
// This translation unit is included from `gpr_processor.cpp` *after*
|
||||
// `gpr_backprojection_processor.ipp`, which defines the shared building blocks
|
||||
// (kPi, SelectedTrace, GeometrySelection, distance_3d, fft_inplace, ...). Do
|
||||
// not include this file directly.
|
||||
//
|
||||
// Pipeline overview (mirrors the Python reference Ellips_motion_remake_2.py):
|
||||
// 1. Pre-process each pair: optional background subtraction (already done in
|
||||
// collect_selected_traces); optional intra-sweep phase correction of S21
|
||||
// before the IFFT — compensates radar displacement that happens *inside*
|
||||
// one sweep where the frequencies are stepped linearly in time.
|
||||
// 2. IFFT each pair → A-scan; find peaks above an SNR threshold within the
|
||||
// depth gate; record both raw and attenuation-compensated SNR.
|
||||
// 3. Apply coarse per-event motion correction so every peak carries a
|
||||
// motion-corrected (tau_corr, z_corr) on top of the apparent values.
|
||||
// 4. CLEAN-style iterative ellipse intersection: build a soft Gaussian-shell
|
||||
// accumulator, take the strongest pixel, count agreeing pairs, suppress
|
||||
// its depth band, repeat.
|
||||
// 5. Optional extended-mode region detection for diffuse reflectors.
|
||||
//
|
||||
// Sweep-event model — radar topology matters:
|
||||
// * Matrix radars (`librevna_multi`, `sn9000`) fire one Tx at a time and
|
||||
// receive on all Rx channels in parallel. A run of 8 traces is just 2
|
||||
// Tx-events; all (tx_k, *) pairs share one timestamp.
|
||||
// * Sequential radars (single librevna with switches, kamil_adc, k209…)
|
||||
// measure each pair separately. A run of 8 traces is 8 separate sweeps;
|
||||
// every pair has its own timestamp.
|
||||
// Both cases reduce to: there are N events in the frame, each event lasts
|
||||
// `event_duration_s = (capture_end_ns - capture_start_ns) / N`. What changes
|
||||
// is how event indices are assigned to pairs — by Tx for matrix radars, by
|
||||
// trace run-order for sequential ones. `event_duration_s` is derived from
|
||||
// the collection metadata, never from a live-config knob.
|
||||
|
||||
constexpr double kLegacyGridZMinM = 0.20;
|
||||
constexpr double kLegacySmoothSigma = 3.0;
|
||||
constexpr double kLegacyCleanSuppressRadiusM = 0.07;
|
||||
@@ -5,6 +39,7 @@ constexpr double kLegacyCleanThresholdFrac = 0.05;
|
||||
constexpr std::size_t kLegacyMaxObjects = 15U;
|
||||
constexpr double kLegacyExtendedThresholdFrac = 0.75;
|
||||
constexpr double kLegacyExtendedMinAreaCm2 = 2.0;
|
||||
constexpr double kLegacyAttenuationReferenceDepthM = 3.0;
|
||||
|
||||
struct LegacyAscanResult {
|
||||
std::vector<double> time_s{};
|
||||
@@ -36,8 +71,26 @@ struct LegacyRegionRecord {
|
||||
std::vector<float> mask{};
|
||||
};
|
||||
|
||||
struct LegacyPairTiming {
|
||||
double dtau_motion_s = 0.0;
|
||||
// Per-pair timing snapshot. For matrix radars, all (tx_k, *) pairs share the
|
||||
// same row; for sequential radars every pair has a distinct row. Indexing by
|
||||
// pair keeps the rest of the pipeline ignorant of the radar topology.
|
||||
struct LegacyEventTiming {
|
||||
std::size_t event_index = 0U; // 0-based order of the event in the frame
|
||||
double t_start_s = 0.0; // start of this event sweep relative to frame
|
||||
double t_center_s = 0.0; // center of this event sweep
|
||||
double dt_ref_s = 0.0; // t_center_s - t_frame_ref_s
|
||||
double dz_motion_m = 0.0; // direction_sign * speed * dt_ref * cos(theta)
|
||||
double dtau_motion_s = 0.0; // 2 * dz_motion_m / velocity
|
||||
};
|
||||
|
||||
struct LegacyMotionTiming {
|
||||
double event_duration_s = 0.0; // (capture_end - capture_start) / num_events
|
||||
double cos_look_angle = 1.0;
|
||||
double direction_sign = 1.0;
|
||||
double speed_m_s = 0.0;
|
||||
bool apply_intra_sweep_phase = false;
|
||||
bool parallel_rx_per_tx_event = false;
|
||||
std::unordered_map<PairKey, LegacyEventTiming> by_pair{};
|
||||
};
|
||||
|
||||
enum class LegacyPeakDomain {
|
||||
@@ -90,20 +143,31 @@ enum class LegacyPeakDomain {
|
||||
return selected;
|
||||
}
|
||||
|
||||
// Expected geo*pattern attenuation for a target directly under the virtual
|
||||
// pair center at depth `z_app`. Uses full 3D antenna coordinates so this
|
||||
// generalizes to non-coplanar antenna layouts; boresight is taken along +Z.
|
||||
[[nodiscard]] auto legacy_attenuation_at_depth(
|
||||
std::size_t tx_index,
|
||||
std::size_t rx_index,
|
||||
double z_app,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx
|
||||
const GeometrySelection& selection,
|
||||
double imaging_plane_y_m
|
||||
) -> double {
|
||||
const double x_center = 0.5 * (x_tx[tx_index] + x_rx[rx_index]);
|
||||
const double r_tx = std::hypot(x_center - x_tx[tx_index], z_app);
|
||||
const double r_rx = std::hypot(x_center - x_rx[rx_index], z_app);
|
||||
const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
|
||||
const double r_tx = distance_3d(
|
||||
x_center - selection.x_tx[tx_index],
|
||||
imaging_plane_y_m - selection.y_tx[tx_index],
|
||||
z_app - selection.z_tx[tx_index]
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
x_center - selection.x_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
z_app - selection.z_rx[rx_index]
|
||||
);
|
||||
const double cos_tx = (z_app - selection.z_tx[tx_index]) / (r_tx + 1e-12);
|
||||
const double cos_rx = (z_app - selection.z_rx[rx_index]) / (r_rx + 1e-12);
|
||||
const double geo = 1.0 / ((r_tx * r_rx) + 1e-12);
|
||||
const double pattern =
|
||||
std::pow(z_app / (r_tx + 1e-12), 2.0) *
|
||||
std::pow(z_app / (r_rx + 1e-12), 2.0);
|
||||
const double pattern = (cos_tx * cos_tx) * (cos_rx * cos_rx);
|
||||
return (geo * pattern) + 1e-30;
|
||||
}
|
||||
|
||||
@@ -203,67 +267,193 @@ enum class LegacyPeakDomain {
|
||||
return false;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_legacy_motion_timing_by_pair(
|
||||
const std::vector<SelectedTrace>& traces,
|
||||
std::size_t total_combo_count,
|
||||
// True when `z_value` should participate in the ellipse vote — both the static
|
||||
// depth gate ([min, max]) and the per-step CLEAN-suppression bands must allow it.
|
||||
// Mirrors Python's `_peak_in_work_depth` + the per-step `excl_z` filter.
|
||||
[[nodiscard]] auto is_legacy_depth_active(
|
||||
double z_value,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
const std::vector<std::pair<double, double>>& excluded_ranges
|
||||
) -> bool {
|
||||
if (z_value < min_depth_m || z_value > max_depth_m) {
|
||||
return false;
|
||||
}
|
||||
return !is_legacy_depth_excluded(z_value, excluded_ranges);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_matrix_radar_model(const std::string& model) -> bool {
|
||||
return model == "librevna_multi" || model == "sn9000";
|
||||
}
|
||||
|
||||
// Assign an `event_index` to every selected pair. The mapping depends on the
|
||||
// radar topology:
|
||||
// * Matrix radar — all (tx_k, *) pairs share one event, ordered by the Tx's
|
||||
// first appearance in run order. So 8 traces with 2 Tx's give 2 events.
|
||||
// * Sequential radar — every pair is its own event, ordered by run order.
|
||||
// So 8 traces give 8 events.
|
||||
[[nodiscard]] auto assign_event_indices(
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
bool matrix_radar
|
||||
) -> std::pair<std::unordered_map<PairKey, std::size_t>, std::size_t> {
|
||||
std::unordered_map<PairKey, std::size_t> event_index_by_pair{};
|
||||
event_index_by_pair.reserve(selected_traces.size());
|
||||
|
||||
if (matrix_radar) {
|
||||
std::unordered_map<std::uint32_t, std::size_t> event_by_tx{};
|
||||
std::size_t next_event = 0U;
|
||||
for (const auto& trace : selected_traces) {
|
||||
const auto [event_it, inserted] = event_by_tx.try_emplace(trace.tx_local_index, next_event);
|
||||
if (inserted) {
|
||||
++next_event;
|
||||
}
|
||||
event_index_by_pair[make_pair_key(trace.tx_local_index, trace.rx_local_index)] = event_it->second;
|
||||
}
|
||||
return {std::move(event_index_by_pair), next_event};
|
||||
}
|
||||
|
||||
// Sequential mode: rank traces by their run_order so the event index is a
|
||||
// dense 0..N-1 sequence regardless of any holes in run_order.
|
||||
std::vector<std::pair<std::size_t, PairKey>> ordered{};
|
||||
ordered.reserve(selected_traces.size());
|
||||
for (const auto& trace : selected_traces) {
|
||||
ordered.emplace_back(trace.run_order, make_pair_key(trace.tx_local_index, trace.rx_local_index));
|
||||
}
|
||||
std::sort(ordered.begin(), ordered.end(),
|
||||
[](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
|
||||
for (std::size_t event_index = 0U; event_index < ordered.size(); ++event_index) {
|
||||
event_index_by_pair[ordered[event_index].second] = event_index;
|
||||
}
|
||||
return {std::move(event_index_by_pair), ordered.size()};
|
||||
}
|
||||
|
||||
// Build per-pair motion timing. `event_duration_s` is derived from collection
|
||||
// metadata as `(capture_end_ns - capture_start_ns) / num_events` — it is the
|
||||
// duration of one sweep event in the frame, never a live-config knob. If the
|
||||
// motion model is disabled (speed = 0 and phase correction off), the function
|
||||
// still returns one row per pair so downstream code can index uniformly.
|
||||
[[nodiscard]] auto compute_legacy_motion_timing(
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
bool matrix_radar,
|
||||
std::uint64_t capture_start_ns,
|
||||
std::uint64_t capture_end_ns,
|
||||
const ProcessingLiveConfig& live_config,
|
||||
double velocity_mps
|
||||
) -> std::unordered_map<PairKey, LegacyPairTiming> {
|
||||
std::unordered_map<PairKey, LegacyPairTiming> timing_by_pair{};
|
||||
timing_by_pair.reserve(traces.size());
|
||||
) -> LegacyMotionTiming {
|
||||
LegacyMotionTiming timing{};
|
||||
timing.speed_m_s = static_cast<double>(live_config.gpr_speed_m_s);
|
||||
timing.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
|
||||
timing.cos_look_angle = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
|
||||
timing.apply_intra_sweep_phase = live_config.gpr_apply_freq_phase_correction;
|
||||
timing.parallel_rx_per_tx_event = matrix_radar;
|
||||
|
||||
const double speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
|
||||
if (!(std::abs(speed_mps) > 1e-12)) {
|
||||
for (const auto& trace : traces) {
|
||||
timing_by_pair.emplace(make_pair_key(trace.tx_local_index, trace.rx_local_index), LegacyPairTiming{});
|
||||
}
|
||||
return timing_by_pair;
|
||||
if (selected_traces.empty()) {
|
||||
return timing;
|
||||
}
|
||||
|
||||
if (total_combo_count == 0U) {
|
||||
throw std::runtime_error("Legacy GPR requires at least one run combo");
|
||||
auto [event_index_by_pair, num_events] = assign_event_indices(selected_traces, matrix_radar);
|
||||
if (num_events == 0U) {
|
||||
return timing;
|
||||
}
|
||||
|
||||
const bool speed_meaningful = std::abs(timing.speed_m_s) > 1e-12;
|
||||
const bool model_active = speed_meaningful || timing.apply_intra_sweep_phase;
|
||||
|
||||
// No motion and no phase correction — populate with zeroed rows and bail.
|
||||
if (!model_active) {
|
||||
for (const auto& [pair_key, event_index] : event_index_by_pair) {
|
||||
timing.by_pair.emplace(pair_key, LegacyEventTiming{.event_index = event_index});
|
||||
}
|
||||
return timing;
|
||||
}
|
||||
|
||||
if (capture_end_ns <= capture_start_ns) {
|
||||
throw std::runtime_error(
|
||||
"Legacy GPR requires valid capture_start_ns/capture_end_ns metadata when speed is non-zero"
|
||||
"Legacy GPR motion model requires valid capture_start_ns/capture_end_ns metadata"
|
||||
);
|
||||
}
|
||||
|
||||
const double capture_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
|
||||
const double slot_duration_s = capture_span_s / static_cast<double>(total_combo_count);
|
||||
if (!(slot_duration_s > 0.0)) {
|
||||
throw std::runtime_error("Legacy GPR requires positive collection capture span when speed is non-zero");
|
||||
const double total_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
|
||||
const double event_duration_s = total_span_s / static_cast<double>(num_events);
|
||||
if (!(event_duration_s > 0.0)) {
|
||||
throw std::runtime_error("Legacy GPR motion model requires positive per-event duration");
|
||||
}
|
||||
timing.event_duration_s = event_duration_s;
|
||||
|
||||
// Reference anchor for dt_ref: either the midpoint between the first and
|
||||
// last event centers (frame_center) or just the first event center
|
||||
// (first_tx_event). Matches Python's `MOTION_CONFIG.reference_mode`.
|
||||
const double first_center_s = 0.5 * event_duration_s;
|
||||
const double last_center_s = (static_cast<double>(num_events) - 0.5) * event_duration_s;
|
||||
const double t_ref_s = live_config.gpr_reference_mode == LegacyGprReferenceMode::FirstTxEvent
|
||||
? first_center_s
|
||||
: 0.5 * (first_center_s + last_center_s);
|
||||
|
||||
const double motion_factor = speed_meaningful
|
||||
? timing.direction_sign * timing.speed_m_s * timing.cos_look_angle
|
||||
: 0.0;
|
||||
|
||||
for (const auto& [pair_key, event_index] : event_index_by_pair) {
|
||||
LegacyEventTiming row{};
|
||||
row.event_index = event_index;
|
||||
row.t_start_s = static_cast<double>(event_index) * event_duration_s;
|
||||
row.t_center_s = row.t_start_s + (0.5 * event_duration_s);
|
||||
row.dt_ref_s = row.t_center_s - t_ref_s;
|
||||
row.dz_motion_m = motion_factor * row.dt_ref_s;
|
||||
row.dtau_motion_s = (2.0 * row.dz_motion_m) / velocity_mps;
|
||||
timing.by_pair.emplace(pair_key, row);
|
||||
}
|
||||
|
||||
const double t_ref_s = 0.5 * capture_span_s;
|
||||
const double cos_theta = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
|
||||
for (const auto& trace : traces) {
|
||||
const double t_center_s = (static_cast<double>(trace.run_order) + 0.5) * slot_duration_s;
|
||||
const double dz_motion_m = speed_mps * (t_center_s - t_ref_s) * cos_theta;
|
||||
timing_by_pair.emplace(
|
||||
make_pair_key(trace.tx_local_index, trace.rx_local_index),
|
||||
LegacyPairTiming{.dtau_motion_s = (2.0 * dz_motion_m) / velocity_mps}
|
||||
);
|
||||
return timing;
|
||||
}
|
||||
|
||||
// Compensate for the radar moving while a single sweep is being recorded.
|
||||
// Frequencies inside one sweep are stepped linearly in time, so each frequency
|
||||
// is sampled from a slightly different antenna position. The correction shifts
|
||||
// each frequency's phase back to the event center; after that the IFFT
|
||||
// produces an A-scan as if the whole sweep were captured at one position.
|
||||
void apply_intra_sweep_phase_correction(
|
||||
SelectedTrace& trace,
|
||||
const LegacyEventTiming& timing,
|
||||
const LegacyMotionTiming& motion,
|
||||
double velocity_mps
|
||||
) {
|
||||
if (!motion.apply_intra_sweep_phase || !(motion.event_duration_s > 0.0)) {
|
||||
return;
|
||||
}
|
||||
if (!(std::abs(motion.speed_m_s) > 1e-12)) {
|
||||
return; // No motion → zero phase shift, no-op.
|
||||
}
|
||||
const std::size_t point_count = trace.frequency_hz.size();
|
||||
if (point_count < 2U || trace.s21.size() != point_count) {
|
||||
return;
|
||||
}
|
||||
|
||||
return timing_by_pair;
|
||||
const double dt_freq_s = motion.event_duration_s / static_cast<double>(point_count - 1U);
|
||||
const double motion_factor = motion.direction_sign * motion.speed_m_s * motion.cos_look_angle;
|
||||
|
||||
for (std::size_t index = 0U; index < point_count; ++index) {
|
||||
const double t_abs_s = timing.t_start_s + (static_cast<double>(index) * dt_freq_s);
|
||||
const double dt_intra_s = t_abs_s - timing.t_center_s;
|
||||
const double delta_path_m = 2.0 * motion_factor * dt_intra_s;
|
||||
const double phi = (2.0 * kPi * trace.frequency_hz[index] * delta_path_m) / velocity_mps;
|
||||
trace.s21[index] *= std::polar(1.0, phi);
|
||||
}
|
||||
}
|
||||
|
||||
void apply_legacy_motion_correction(
|
||||
std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::unordered_map<PairKey, LegacyPairTiming>& timing_by_pair,
|
||||
const LegacyMotionTiming& motion,
|
||||
double velocity_mps
|
||||
) {
|
||||
for (auto& [key, peaks] : peaks_by_pair) {
|
||||
const auto timing_it = timing_by_pair.find(key);
|
||||
if (timing_it == timing_by_pair.end()) {
|
||||
throw std::runtime_error("Missing motion timing for selected legacy GPR combo");
|
||||
const auto timing_it = motion.by_pair.find(key);
|
||||
if (timing_it == motion.by_pair.end()) {
|
||||
throw std::runtime_error("Missing motion timing for selected legacy GPR pair");
|
||||
}
|
||||
|
||||
const double dtau_motion_s = timing_it->second.dtau_motion_s;
|
||||
for (auto& peak : peaks) {
|
||||
peak.tau_corr = peak.tau + timing_it->second.dtau_motion_s;
|
||||
peak.tau_corr = peak.tau + dtau_motion_s;
|
||||
peak.z_corr = 0.5 * velocity_mps * peak.tau_corr;
|
||||
}
|
||||
}
|
||||
@@ -275,6 +465,8 @@ void apply_legacy_motion_correction(
|
||||
const std::vector<std::pair<double, double>>& exclude_ranges,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
LegacyPeakDomain domain
|
||||
@@ -298,7 +490,8 @@ void apply_legacy_motion_correction(
|
||||
const auto& tx_grid = grid.tx_distance_grids[tx_index];
|
||||
const auto& rx_grid = grid.rx_distance_grids[rx_index];
|
||||
for (const auto& peak : peak_it->second) {
|
||||
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
|
||||
const double depth = legacy_peak_depth_for_domain(peak, domain);
|
||||
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -324,6 +517,8 @@ void apply_legacy_motion_correction(
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
LegacyPeakDomain domain
|
||||
) -> double {
|
||||
std::size_t count = 0U;
|
||||
@@ -337,7 +532,8 @@ void apply_legacy_motion_correction(
|
||||
}
|
||||
|
||||
for (const auto& peak : peak_it->second) {
|
||||
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
|
||||
const double depth = legacy_peak_depth_for_domain(peak, domain);
|
||||
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -405,22 +601,26 @@ void apply_legacy_motion_correction(
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
LegacyPeakDomain domain
|
||||
) -> std::pair<std::vector<LegacyPointRecord>, std::vector<double>> {
|
||||
std::vector<LegacyPointRecord> found{};
|
||||
const auto accumulator =
|
||||
build_legacy_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx, domain);
|
||||
const auto accumulator = build_legacy_accumulator(
|
||||
grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, min_depth_m, max_depth_m, x_tx, x_rx, domain
|
||||
);
|
||||
const double initial_max = max_value(accumulator);
|
||||
if (!(initial_max > 0.0) || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) {
|
||||
return {found, gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kLegacySmoothSigma)};
|
||||
}
|
||||
|
||||
// Suppression radius rounds down to mirror Python's `int(0.07 / dx)`.
|
||||
const double dx = grid.x_grid[1] - grid.x_grid[0];
|
||||
const double dz = grid.z_grid[1] - grid.z_grid[0];
|
||||
const auto radius_x =
|
||||
static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
|
||||
static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
|
||||
const auto radius_z =
|
||||
static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
|
||||
static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
|
||||
|
||||
std::vector<std::pair<double, double>> excluded_ranges{};
|
||||
for (std::size_t step = 0U; step < kLegacyMaxObjects; ++step) {
|
||||
@@ -430,6 +630,8 @@ void apply_legacy_motion_correction(
|
||||
excluded_ranges,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
x_tx,
|
||||
x_rx,
|
||||
domain
|
||||
@@ -469,11 +671,16 @@ void apply_legacy_motion_correction(
|
||||
x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
domain
|
||||
),
|
||||
}
|
||||
);
|
||||
|
||||
// Collect depths of all peaks consistent with the just-detected point;
|
||||
// they form the next exclusion band so subsequent CLEAN steps cannot
|
||||
// re-pick the same target.
|
||||
std::vector<double> matched_depths{};
|
||||
for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) {
|
||||
for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) {
|
||||
@@ -484,14 +691,15 @@ void apply_legacy_motion_correction(
|
||||
continue;
|
||||
}
|
||||
for (const auto& peak : peak_it->second) {
|
||||
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), excluded_ranges)) {
|
||||
const double depth = legacy_peak_depth_for_domain(peak, domain);
|
||||
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, excluded_ranges)) {
|
||||
continue;
|
||||
}
|
||||
const double r_tx = std::hypot(x_est - x_tx[tx_index], z_est);
|
||||
const double r_rx = std::hypot(x_est - x_rx[rx_index], z_est);
|
||||
if (std::abs((r_tx + r_rx) - (velocity_mps * legacy_peak_tau_for_domain(peak, domain))) <
|
||||
shell_sigma_m * 3.0) {
|
||||
matched_depths.push_back(legacy_peak_depth_for_domain(peak, domain));
|
||||
matched_depths.push_back(depth);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -512,7 +720,9 @@ void apply_legacy_motion_correction(
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m
|
||||
double shell_sigma_m,
|
||||
double min_depth_m,
|
||||
double max_depth_m
|
||||
) -> std::pair<std::vector<LegacyRegionRecord>, std::vector<double>> {
|
||||
std::vector<LegacyRegionRecord> regions{};
|
||||
const auto accumulator = build_legacy_accumulator(
|
||||
@@ -521,6 +731,8 @@ void apply_legacy_motion_correction(
|
||||
{},
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
x_tx,
|
||||
x_rx,
|
||||
LegacyPeakDomain::Apparent
|
||||
@@ -622,6 +834,8 @@ void apply_legacy_motion_correction(
|
||||
x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
LegacyPeakDomain::Apparent
|
||||
);
|
||||
region.pixel_count = static_cast<double>(component.size());
|
||||
@@ -649,7 +863,7 @@ void apply_legacy_motion_correction(
|
||||
|
||||
validate_collection_trace_order(run_config, collection);
|
||||
const auto background_mean = build_background_mean(previous_collections, selection, live_config);
|
||||
const auto selected_traces = collect_selected_traces(collection, selection, background_mean);
|
||||
auto selected_traces = collect_selected_traces(collection, selection, background_mean);
|
||||
if (selected_traces.empty()) {
|
||||
return results;
|
||||
}
|
||||
@@ -663,6 +877,32 @@ void apply_legacy_motion_correction(
|
||||
if (!(max_depth_m > min_depth_m)) {
|
||||
return results;
|
||||
}
|
||||
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
|
||||
|
||||
// Motion timing is computed once per collection. Matrix radars get one
|
||||
// event per Tx (parallel Rx); sequential radars get one event per pair.
|
||||
const bool matrix_radar = is_matrix_radar_model(run_config.radar.model);
|
||||
const auto motion_timing = compute_legacy_motion_timing(
|
||||
selected_traces,
|
||||
matrix_radar,
|
||||
collection.capture_start_ns,
|
||||
collection.capture_end_ns,
|
||||
live_config,
|
||||
velocity_mps
|
||||
);
|
||||
|
||||
// Intra-sweep phase correction (frequency-domain) — happens BEFORE the IFFT
|
||||
// because it modifies the S21 spectrum that compute_legacy_ascan consumes.
|
||||
if (motion_timing.apply_intra_sweep_phase) {
|
||||
for (auto& trace : selected_traces) {
|
||||
const auto pair_key = make_pair_key(trace.tx_local_index, trace.rx_local_index);
|
||||
const auto timing_it = motion_timing.by_pair.find(pair_key);
|
||||
if (timing_it == motion_timing.by_pair.end()) {
|
||||
continue;
|
||||
}
|
||||
apply_intra_sweep_phase_correction(trace, timing_it->second, motion_timing, velocity_mps);
|
||||
}
|
||||
}
|
||||
|
||||
std::unordered_map<PairKey, LegacyAscanResult> ascans_by_pair{};
|
||||
double bandwidth_hz = 0.0;
|
||||
@@ -678,12 +918,7 @@ void apply_legacy_motion_correction(
|
||||
return results;
|
||||
}
|
||||
|
||||
const auto grid = build_grid(
|
||||
selection,
|
||||
max_depth_m,
|
||||
kLegacyGridZMinM,
|
||||
static_cast<double>(live_config.gpr_imaging_plane_y_m)
|
||||
);
|
||||
const auto grid = build_grid(selection, max_depth_m, kLegacyGridZMinM, imaging_plane_y_m);
|
||||
if (grid.x_grid.empty() || grid.z_grid.empty()) {
|
||||
return results;
|
||||
}
|
||||
@@ -724,15 +959,17 @@ void apply_legacy_motion_correction(
|
||||
const auto peak_indices =
|
||||
find_legacy_peak_indices(ascan.amplitude, min_index, max_index, noise * snr_thresh, min_distance);
|
||||
|
||||
const double attenuation_ref = legacy_attenuation_at_depth(
|
||||
tx_index, rx_index, kLegacyAttenuationReferenceDepthM, selection, imaging_plane_y_m
|
||||
);
|
||||
|
||||
auto& peaks = peaks_by_pair[key];
|
||||
peaks.reserve(peak_indices.size());
|
||||
for (const auto peak_index : peak_indices) {
|
||||
const double z_app = ascan.depth_m[peak_index];
|
||||
const double snr_raw = ascan.amplitude[peak_index] / std::max(noise, 1e-12);
|
||||
const double attenuation =
|
||||
legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection.x_tx, selection.x_rx);
|
||||
const double attenuation_ref =
|
||||
legacy_attenuation_at_depth(tx_index, rx_index, 3.0, selection.x_tx, selection.x_rx);
|
||||
legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection, imaging_plane_y_m);
|
||||
const double snr_comp = std::min(
|
||||
snr_raw / (std::pow(attenuation / attenuation_ref, comp_power) + 1e-12),
|
||||
snr_comp_max
|
||||
@@ -762,7 +999,9 @@ void apply_legacy_motion_correction(
|
||||
selection.x_tx,
|
||||
selection.x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m
|
||||
);
|
||||
results.collection_payloads.push_back(
|
||||
build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator)
|
||||
@@ -793,15 +1032,10 @@ void apply_legacy_motion_correction(
|
||||
return results;
|
||||
}
|
||||
|
||||
const auto motion_timing_by_pair = build_legacy_motion_timing_by_pair(
|
||||
selected_traces,
|
||||
run_config.run_combos.size(),
|
||||
collection.capture_start_ns,
|
||||
collection.capture_end_ns,
|
||||
live_config,
|
||||
velocity_mps
|
||||
);
|
||||
apply_legacy_motion_correction(peaks_by_pair, motion_timing_by_pair, velocity_mps);
|
||||
// Apply coarse per-Tx-event motion correction to the peak set produced
|
||||
// above. After this step every peak carries both apparent and motion-
|
||||
// corrected (tau, depth) values; the CLEAN search uses the corrected domain.
|
||||
apply_legacy_motion_correction(peaks_by_pair, motion_timing, velocity_mps);
|
||||
|
||||
const auto [points, smoothed_accumulator] = clean_legacy_find_points(
|
||||
grid,
|
||||
@@ -810,6 +1044,8 @@ void apply_legacy_motion_correction(
|
||||
selection.x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
LegacyPeakDomain::Corrected
|
||||
);
|
||||
results.collection_payloads.push_back(
|
||||
|
||||
@@ -34,6 +34,19 @@ class RadarDriver {
|
||||
virtual void close() = 0;
|
||||
/** @brief Acquire one sweep containing the forward traces exposed by the driver. */
|
||||
[[nodiscard]] virtual auto acquire_sweep() -> SweepTrace = 0;
|
||||
|
||||
/**
|
||||
* @brief Announce which switch combo the next `acquire_sweep` belongs to.
|
||||
*
|
||||
* Real radars are agnostic to this because the switch state itself decides
|
||||
* what they see. Mock drivers use it to synthesise per-combo variation so
|
||||
* downstream plots show eight distinct traces for an eight-combo run
|
||||
* instead of eight identical curves stacked on top of each other.
|
||||
*
|
||||
* Default implementation is a no-op so production drivers do not need to
|
||||
* override.
|
||||
*/
|
||||
virtual void set_active_combo(const ipc::ComboKey& /*combo*/) {}
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
|
||||
+18
-3
@@ -126,6 +126,10 @@ void LibreVnaMinimalDriver::close() {
|
||||
is_open_ = false;
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::set_active_combo(const ipc::ComboKey& combo) {
|
||||
active_combo_ = combo;
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::acquire_sweep() -> SweepTrace {
|
||||
if (!is_open_) {
|
||||
throw std::runtime_error("Radar driver is not open");
|
||||
@@ -189,6 +193,17 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
|
||||
const float range_drift_m =
|
||||
0.01F * std::sin(0.07F * static_cast<float>(sweep_index_));
|
||||
|
||||
// Combo-dependent variation. Without this the mock returns near-identical
|
||||
// S21 for every (input, output) combo and an eight-combo pass-through plot
|
||||
// collapses into a single visible trace. The factors below are arbitrary
|
||||
// but chosen small enough that the overall response stays in a reasonable
|
||||
// band and large enough that each pair is visually distinct.
|
||||
const auto input_pos = static_cast<float>(active_combo_.input_pos);
|
||||
const auto output_pos = static_cast<float>(active_combo_.output_pos);
|
||||
const float combo_amplitude_gain = 0.55F + 0.08F * input_pos + 0.05F * output_pos;
|
||||
const float combo_phase_offset = 0.4F * input_pos + 0.9F * output_pos;
|
||||
const float combo_range_offset_m = 0.05F * input_pos + 0.12F * output_pos;
|
||||
|
||||
// Deterministic-per-sweep noise so two consecutive frames look distinct
|
||||
// but the test stays reproducible for any given sweep index.
|
||||
std::mt19937 noise_engine(
|
||||
@@ -205,7 +220,7 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
|
||||
std::complex<float> s11_total{0.0F, 0.0F};
|
||||
|
||||
for (const auto& target : kMockTargets) {
|
||||
const float range_m = target.range_m + range_drift_m;
|
||||
const float range_m = target.range_m + range_drift_m + combo_range_offset_m;
|
||||
// Round-trip phase: 2π·f·(2R/v).
|
||||
const float round_trip_phase =
|
||||
2.0F * detail::kPi * frequency_hz * (2.0F * range_m / kGroundVelocityMps);
|
||||
@@ -216,8 +231,8 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
|
||||
std::exp(-kAttenuationCoeffPerMeterAtRefHz * range_m * frequency_scale);
|
||||
|
||||
const std::complex<float> contribution = std::polar<float>(
|
||||
target.reflection_magnitude * spreading * attenuation,
|
||||
-round_trip_phase
|
||||
target.reflection_magnitude * spreading * attenuation * combo_amplitude_gain,
|
||||
-round_trip_phase + combo_phase_offset
|
||||
);
|
||||
s21_total += contribution;
|
||||
s11_total += kS11CrossCouplingFactor * contribution;
|
||||
|
||||
+5
@@ -43,6 +43,7 @@ class LibreVnaMinimalDriver final : public RadarDriver {
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto acquire_sweep() -> SweepTrace override;
|
||||
void set_active_combo(const ipc::ComboKey& combo) override;
|
||||
|
||||
private:
|
||||
/**
|
||||
@@ -92,6 +93,10 @@ class LibreVnaMinimalDriver final : public RadarDriver {
|
||||
LibreVnaMinimalDriverSettings settings_{};
|
||||
bool is_open_ = false;
|
||||
std::uint64_t sweep_index_ = 0;
|
||||
// Latest combo announced by the orchestrator. Used by the mock backend to
|
||||
// give each (input, output) pair a slightly different reflectivity profile
|
||||
// so a multi-combo run does not render as eight identical traces.
|
||||
ipc::ComboKey active_combo_{};
|
||||
|
||||
libusb_context* usb_context_ = nullptr;
|
||||
libusb_device_handle* usb_handle_ = nullptr;
|
||||
|
||||
@@ -157,6 +157,9 @@ auto SweepOrchestrator::acquire_one_collection(
|
||||
input_switch_driver_.switch_to(combo.input_pos);
|
||||
sleep_if_needed_ms(config_.runtime.settling_ms);
|
||||
|
||||
// Production drivers ignore this; mock drivers use it to give every
|
||||
// (input, output) pair its own synthetic response.
|
||||
radar_driver_.set_active_combo(combo);
|
||||
auto sweep = radar_driver_.acquire_sweep();
|
||||
validate_sweep(sweep);
|
||||
|
||||
|
||||
@@ -169,14 +169,15 @@ class AppWindow(
|
||||
|
||||
def _init_history_state(self) -> None:
|
||||
"""Initialize runtime history buffers and render-cache state."""
|
||||
history_limit = self._history_limit_from_config()
|
||||
self._raw_history: deque[SweepCollection] = deque(maxlen=history_limit)
|
||||
self._pre_history: deque[SweepCollection] = deque(maxlen=history_limit)
|
||||
self._result_history: deque[ResultCollection] = deque(maxlen=history_limit)
|
||||
bscan_history_limit = self._history_limit_from_config()
|
||||
save_history_limit = self._save_history_limit_from_config()
|
||||
self._raw_history: deque[SweepCollection] = deque(maxlen=save_history_limit)
|
||||
self._pre_history: deque[SweepCollection] = deque(maxlen=save_history_limit)
|
||||
self._result_history: deque[ResultCollection] = deque(maxlen=save_history_limit)
|
||||
|
||||
# Sequence id must survive GUI restarts so history commands stay monotonic.
|
||||
self._history_command_seq = self._load_history_command_seq(self._live_config_writer.path)
|
||||
self._bscan_history_limit = history_limit
|
||||
self._bscan_history_limit = bscan_history_limit
|
||||
self._bscan_history_by_combo = {}
|
||||
self._bscan_depth_axis_by_combo = {}
|
||||
self._bscan_history_floor_collection_id = 0
|
||||
@@ -200,15 +201,12 @@ class AppWindow(
|
||||
self._radar_limits: dict[str, float | int] | None = None
|
||||
|
||||
def _history_limit_from_config(self) -> int:
|
||||
"""Return unified GUI history limit derived from configured ring capacities."""
|
||||
return max(
|
||||
1,
|
||||
min(
|
||||
int(self._defaults_config.rings.raw_tap.capacity),
|
||||
int(self._defaults_config.rings.preprocessed_tap.capacity),
|
||||
int(self._defaults_config.rings.results.capacity),
|
||||
),
|
||||
)
|
||||
"""Return B-scan render history limit derived from configured ring capacities."""
|
||||
return self._history_limit_for_config(self._defaults_config)
|
||||
|
||||
def _save_history_limit_from_config(self) -> int:
|
||||
"""Return maxlen for GUI snapshot-save deques (independent of ring capacities)."""
|
||||
return self._save_history_limit_for_config(self._defaults_config)
|
||||
|
||||
def _init_runtime_limits(self) -> None:
|
||||
"""Initialize read/drain loop limits used by polling and snapshot code."""
|
||||
|
||||
@@ -81,6 +81,10 @@ class AppWindowLiveProcessingMixin:
|
||||
gpr_draw_top_m_objects=int(self._gpr_draw_top_m_objects.value()),
|
||||
gpr_speed_m_s=float(self._legacy_gpr_speed_m_s.value()),
|
||||
gpr_look_angle_deg=float(self._legacy_gpr_look_angle_deg.value()),
|
||||
gpr_apply_freq_phase_correction=bool(
|
||||
self._legacy_gpr_apply_freq_phase_correction.isChecked()
|
||||
),
|
||||
gpr_reference_mode=self._legacy_gpr_reference_mode.currentText(),
|
||||
gpr_snr_thresh=float(self._legacy_gpr_snr_thresh.value()),
|
||||
gpr_snr_comp_max=float(self._legacy_gpr_snr_comp_max.value()),
|
||||
gpr_start_freq_mhz=gpr_start_freq_mhz,
|
||||
|
||||
@@ -117,12 +117,19 @@ class AppWindowConfigProfileIOMixin:
|
||||
self._optical_variation_panel.setVisible(mode == "variation")
|
||||
|
||||
def _apply_history_limit_from_config(self, config) -> None:
|
||||
"""Resize in-memory history buffers to match the loaded config."""
|
||||
history_limit = self._history_limit_for_config(config)
|
||||
self._raw_history = deque(self._raw_history, maxlen=history_limit)
|
||||
self._pre_history = deque(self._pre_history, maxlen=history_limit)
|
||||
self._result_history = deque(self._result_history, maxlen=history_limit)
|
||||
self._bscan_history_limit = history_limit
|
||||
"""Resize in-memory history buffers to match the loaded config.
|
||||
|
||||
Save-side deques use a config-independent limit so that processing-side
|
||||
ring capacities can stay small without truncating the save buffer. The
|
||||
B-scan render limit still follows ring capacities to keep plot updates
|
||||
responsive.
|
||||
"""
|
||||
save_history_limit = self._save_history_limit_for_config(config)
|
||||
bscan_history_limit = self._history_limit_for_config(config)
|
||||
self._raw_history = deque(self._raw_history, maxlen=save_history_limit)
|
||||
self._pre_history = deque(self._pre_history, maxlen=save_history_limit)
|
||||
self._result_history = deque(self._result_history, maxlen=save_history_limit)
|
||||
self._bscan_history_limit = bscan_history_limit
|
||||
self._clear_bscan_plot_history()
|
||||
|
||||
def _save_current_config(self) -> None:
|
||||
@@ -283,6 +290,8 @@ class AppWindowConfigProfileIOMixin:
|
||||
self._legacy_gpr_speed_m_s,
|
||||
self._legacy_gpr_ignore_socket_speed_enabled,
|
||||
self._legacy_gpr_look_angle_deg,
|
||||
self._legacy_gpr_apply_freq_phase_correction,
|
||||
self._legacy_gpr_reference_mode,
|
||||
self._legacy_gpr_background_subtract_enabled,
|
||||
self._legacy_gpr_background_mean_count,
|
||||
self._legacy_gpr_render_mode,
|
||||
@@ -451,6 +460,13 @@ class AppWindowConfigProfileIOMixin:
|
||||
self._legacy_gpr_ignore_socket_speed_enabled.setChecked(ignore_socket_speed_enabled)
|
||||
self._legacy_gpr_speed_m_s.setEnabled(ignore_socket_speed_enabled)
|
||||
self._legacy_gpr_look_angle_deg.setValue(float(gui_state.processing.legacy_gpr.look_angle_deg))
|
||||
self._legacy_gpr_apply_freq_phase_correction.setChecked(
|
||||
bool(gui_state.processing.legacy_gpr.apply_freq_phase_correction)
|
||||
)
|
||||
self._set_combo_current_text(
|
||||
self._legacy_gpr_reference_mode,
|
||||
gui_state.processing.legacy_gpr.reference_mode,
|
||||
)
|
||||
self._legacy_gpr_background_subtract_enabled.setChecked(
|
||||
bool(gui_state.processing.legacy_gpr.background_subtract_enabled)
|
||||
)
|
||||
|
||||
@@ -27,6 +27,14 @@ from python_app.orchestration.preprocess_assets import (
|
||||
from python_app.storage.npz_store import radar_key_from_config
|
||||
|
||||
|
||||
# GUI snapshot-save deques (`_raw_history`, `_pre_history`, `_result_history`)
|
||||
# are intentionally decoupled from SHM ring capacities: rings are sized for the
|
||||
# C++ processing pipeline, while save buffers only retain what the GUI poll
|
||||
# loop already read out. Growing this number lets the user save more recent
|
||||
# history without touching the processing-side ring sizes.
|
||||
GUI_SAVE_HISTORY_LIMIT: int = 1000
|
||||
|
||||
|
||||
class AppWindowConfigStateBuildersMixin:
|
||||
"""Build stable and GUI-only config models from current widget state."""
|
||||
|
||||
@@ -156,7 +164,7 @@ class AppWindowConfigStateBuildersMixin:
|
||||
|
||||
@staticmethod
|
||||
def _history_limit_for_config(config: RunConfigModel) -> int:
|
||||
"""Return unified GUI history limit derived from config ring capacities."""
|
||||
"""Return B-scan render history limit derived from config ring capacities."""
|
||||
return max(
|
||||
1,
|
||||
min(
|
||||
@@ -166,6 +174,18 @@ class AppWindowConfigStateBuildersMixin:
|
||||
),
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _save_history_limit_for_config(config: RunConfigModel) -> int:
|
||||
"""Return maxlen for GUI snapshot-save deques.
|
||||
|
||||
Independent of ring capacities — see :data:`GUI_SAVE_HISTORY_LIMIT`.
|
||||
The `config` argument is kept for symmetry with
|
||||
:meth:`_history_limit_for_config` and possible future per-profile
|
||||
overrides.
|
||||
"""
|
||||
del config
|
||||
return GUI_SAVE_HISTORY_LIMIT
|
||||
|
||||
def _default_gui_state_for_config(self, config: RunConfigModel) -> GuiStateModel:
|
||||
"""Build fallback GUI-only defaults for a stable run config."""
|
||||
default_combo = config.combos[0] if config.combos else ComboModel(input=0, output=0)
|
||||
@@ -352,6 +372,10 @@ class AppWindowConfigStateBuildersMixin:
|
||||
speed_m_s=float(self._legacy_gpr_speed_m_s.value()),
|
||||
ignore_socket_speed_enabled=bool(self._legacy_gpr_ignore_socket_speed_enabled.isChecked()),
|
||||
look_angle_deg=float(self._legacy_gpr_look_angle_deg.value()),
|
||||
apply_freq_phase_correction=bool(
|
||||
self._legacy_gpr_apply_freq_phase_correction.isChecked()
|
||||
),
|
||||
reference_mode=self._legacy_gpr_reference_mode.currentText(),
|
||||
snr_thresh=float(self._legacy_gpr_snr_thresh.value()),
|
||||
snr_comp_max=float(self._legacy_gpr_snr_comp_max.value()),
|
||||
background_subtract_enabled=bool(self._legacy_gpr_background_subtract_enabled.isChecked()),
|
||||
|
||||
@@ -355,7 +355,8 @@ class AppWindowPipelineMixin:
|
||||
|
||||
def _read_all_raw(self) -> SweepCollection | None:
|
||||
"""Read available raw collections from raw ring."""
|
||||
assert self._raw_reader is not None
|
||||
if self._raw_reader is None:
|
||||
raise RuntimeError("Raw ring reader is not initialised")
|
||||
latest: SweepCollection | None = None
|
||||
for _ in range(self._max_pop_per_poll):
|
||||
collection = self._raw_reader.pop_raw_collection()
|
||||
@@ -388,7 +389,8 @@ class AppWindowPipelineMixin:
|
||||
|
||||
def _read_all_results(self) -> ResultCollection | None:
|
||||
"""Read available result collections from results ring."""
|
||||
assert self._result_reader is not None
|
||||
if self._result_reader is None:
|
||||
raise RuntimeError("Result ring reader is not initialised")
|
||||
latest: ResultCollection | None = None
|
||||
for _ in range(self._max_pop_per_poll):
|
||||
collection = self._result_reader.pop_result_collection()
|
||||
|
||||
@@ -403,6 +403,22 @@ def build_processing_group(owner) -> QGroupBox:
|
||||
owner._legacy_gpr_look_angle_deg.setSingleStep(0.1)
|
||||
owner._legacy_gpr_look_angle_deg.setValue(float(legacy_gpr_defaults.look_angle_deg))
|
||||
|
||||
# Intra-sweep phase compensation toggle. When checked, S21 phase is
|
||||
# corrected per frequency point before the IFFT so the radar's motion
|
||||
# during one sweep is removed at the spectrum stage. Only matters when
|
||||
# speed != 0; otherwise has no effect by construction.
|
||||
owner._legacy_gpr_apply_freq_phase_correction = QCheckBox("Apply intra-sweep phase correction")
|
||||
owner._legacy_gpr_apply_freq_phase_correction.setChecked(
|
||||
bool(legacy_gpr_defaults.apply_freq_phase_correction)
|
||||
)
|
||||
|
||||
# Anchor point for the motion model's per-event `dt_ref`. `frame_center`
|
||||
# spreads motion symmetrically around the frame midpoint; `first_tx_event`
|
||||
# accumulates it forward from the first event center.
|
||||
owner._legacy_gpr_reference_mode = QComboBox()
|
||||
owner._legacy_gpr_reference_mode.addItems(["frame_center", "first_tx_event"])
|
||||
owner._set_combo_current_text(owner._legacy_gpr_reference_mode, legacy_gpr_defaults.reference_mode)
|
||||
|
||||
owner._legacy_gpr_background_subtract_enabled = QCheckBox("Subtract mean of previous collections")
|
||||
owner._legacy_gpr_background_subtract_enabled.setChecked(bool(legacy_gpr_defaults.background_subtract_enabled))
|
||||
|
||||
@@ -460,6 +476,8 @@ def build_processing_group(owner) -> QGroupBox:
|
||||
("Start MHz", owner._legacy_gpr_start_freq_mhz),
|
||||
("Stop MHz", owner._legacy_gpr_stop_freq_mhz),
|
||||
("Look angle deg", owner._legacy_gpr_look_angle_deg),
|
||||
("Reference mode", owner._legacy_gpr_reference_mode),
|
||||
owner._legacy_gpr_apply_freq_phase_correction,
|
||||
("Visible X min m", owner._legacy_gpr_visible_x_min_m),
|
||||
("Visible X max m", owner._legacy_gpr_visible_x_max_m),
|
||||
("Visible Z min m", owner._legacy_gpr_visible_z_min_m),
|
||||
@@ -526,6 +544,8 @@ def build_processing_group(owner) -> QGroupBox:
|
||||
owner._on_legacy_gpr_ignore_socket_speed_toggled
|
||||
)
|
||||
owner._legacy_gpr_look_angle_deg.valueChanged.connect(owner._on_processing_live_settings_changed)
|
||||
owner._legacy_gpr_apply_freq_phase_correction.toggled.connect(owner._on_processing_live_settings_changed)
|
||||
owner._legacy_gpr_reference_mode.currentTextChanged.connect(owner._on_processing_live_settings_changed)
|
||||
owner._legacy_gpr_background_subtract_enabled.toggled.connect(owner._on_processing_live_settings_changed)
|
||||
owner._legacy_gpr_background_mean_count.valueChanged.connect(owner._on_processing_live_settings_changed)
|
||||
owner._legacy_gpr_render_mode.currentTextChanged.connect(owner._on_gpr_visual_settings_changed)
|
||||
|
||||
@@ -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,37 +41,30 @@ 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)
|
||||
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:
|
||||
@@ -62,193 +74,222 @@ class KamilAdcFrameParser:
|
||||
|
||||
@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
|
||||
"""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._buffer.clear()
|
||||
self._packet_start_pending = False
|
||||
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:
|
||||
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}")
|
||||
"""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")
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
with self._mailbox_cv:
|
||||
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:
|
||||
# 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:
|
||||
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"
|
||||
f"Timed out waiting for Kamil ADC sweep after {float(timeout_s):.3f}s"
|
||||
)
|
||||
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"
|
||||
# Wake periodically so we can re-check process liveness.
|
||||
self._mailbox_cv.wait(timeout=min(_READ_POLL_INTERVAL_S, remaining_s))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 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 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._validate_and_lock_point_count(len(values))
|
||||
return np.asarray(values, dtype=np.complex64)
|
||||
|
||||
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 _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}"
|
||||
)
|
||||
|
||||
fd = self._require_fd()
|
||||
wait_s = min(0.05, remaining_s)
|
||||
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], [], [], wait_s)
|
||||
readable, _, _ = select.select([fd], [], [], _READ_POLL_INTERVAL_S)
|
||||
except InterruptedError:
|
||||
return
|
||||
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 read 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 reading")
|
||||
self._buffer.extend(chunk)
|
||||
buffer.extend(chunk)
|
||||
return True
|
||||
return False
|
||||
|
||||
def _require_fd(self) -> int:
|
||||
if self._fd is None:
|
||||
raise RuntimeError("Kamil ADC TTY reader is not open")
|
||||
return self._fd
|
||||
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 _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'")
|
||||
|
||||
@@ -216,11 +216,13 @@ class MultiDeviceVnaController:
|
||||
|
||||
def _configure_reference_clocks(self) -> None:
|
||||
for device_connection in self._all_devices:
|
||||
# 1 s ACK timeout plus one retry caps worst-case at ~2 s per device
|
||||
# so a stuck reference apply cannot stall recovery for minutes.
|
||||
self._send_command_and_wait_for_acknowledgement(
|
||||
device_connection,
|
||||
PacketType.REFERENCE_SETTINGS,
|
||||
build_reference_settings_payload(0, self._force_external_reference),
|
||||
timeout_seconds=3.0,
|
||||
timeout_seconds=1.0,
|
||||
retry_count=1,
|
||||
)
|
||||
|
||||
@@ -241,6 +243,8 @@ class MultiDeviceVnaController:
|
||||
(self._master_device, True),
|
||||
]
|
||||
for device_connection, is_synchronization_master in sweep_configuration_commands:
|
||||
# 1 s ACK timeout plus one retry caps worst-case at ~2 s per device
|
||||
# so a stuck sweep apply cannot stall recovery for minutes.
|
||||
self._send_command_and_wait_for_acknowledgement(
|
||||
device_connection,
|
||||
PacketType.SWEEP_SETTINGS,
|
||||
@@ -250,7 +254,7 @@ class MultiDeviceVnaController:
|
||||
synchronization_enabled=self._synchronization_enabled,
|
||||
master_stimulus_ports=master_stimulus_ports,
|
||||
),
|
||||
timeout_seconds=3.0,
|
||||
timeout_seconds=1.0,
|
||||
retry_count=1,
|
||||
)
|
||||
self._last_applied_sweep_configuration = replace(sweep_configuration)
|
||||
|
||||
@@ -23,6 +23,19 @@ from python_app.hardware_full.librevna_multi_device_driver.transport import Libr
|
||||
|
||||
LIBREVNA_NATIVE_SWEEP_TIMEOUT_SECONDS = 1.5
|
||||
|
||||
# Hard upper bound on how long one full sweep cycle is allowed to take from
|
||||
# the moment the collection thread enters its loop. Even a device that keeps
|
||||
# streaming valid-looking datapoints will be abandoned once this deadline
|
||||
# elapses, so the caller's recovery loop can re-open it instead of waiting
|
||||
# forever. Kept comfortably above the worst real-world cycle (≈ points / IFBW).
|
||||
_MAX_FULL_CYCLE_SECONDS = 8.0
|
||||
|
||||
# Maximum time we let collection threads linger after `stop_collection_requested`
|
||||
# has been set. They are all daemon threads and self-poll the flag every
|
||||
# ~0.2 s, so a 2 s grace period is generous. Past this point we stop joining
|
||||
# and let the orphan thread die when the producer process exits.
|
||||
_THREAD_JOIN_TIMEOUT_SECONDS = 2.0
|
||||
|
||||
|
||||
def collect_complete_running_sweep_cycles(
|
||||
*,
|
||||
@@ -82,8 +95,13 @@ def collect_complete_running_sweep_cycles(
|
||||
) -> None:
|
||||
datapoints_received = 0
|
||||
expected_datapoint_count = cycle_count * point_count
|
||||
last_datapoint_timestamp = time.monotonic()
|
||||
collection_loop_start = last_datapoint_timestamp
|
||||
loop_start_timestamp = time.monotonic()
|
||||
# Tracks the last time we actually accepted a datapoint into the cycle.
|
||||
# Crucially, *not* updated on rejected datapoints — a device that keeps
|
||||
# streaming valid-looking frames the handler ignores (e.g. waiting on
|
||||
# point_index=0, or after cycle_count has been reached) must still hit
|
||||
# the per-device timeout and trigger recovery instead of looping forever.
|
||||
last_consumed_timestamp = loop_start_timestamp
|
||||
has_consumed_any_datapoint = False
|
||||
|
||||
while datapoints_received < expected_datapoint_count:
|
||||
@@ -91,19 +109,19 @@ def collect_complete_running_sweep_cycles(
|
||||
return
|
||||
|
||||
now = time.monotonic()
|
||||
remaining_timeout_seconds = (last_datapoint_timestamp + datapoint_timeout_seconds) - now
|
||||
remaining_timeout_seconds = (last_consumed_timestamp + datapoint_timeout_seconds) - now
|
||||
if remaining_timeout_seconds <= 0:
|
||||
collection_errors.append(
|
||||
TimeoutError(
|
||||
f"No datapoints from {device_connection.serial_number} for "
|
||||
f"No usable datapoints from {device_connection.serial_number} for "
|
||||
f"{datapoint_timeout_seconds:.1f} s "
|
||||
f"(received {datapoints_received}/{point_count})"
|
||||
f"(received {datapoints_received}/{expected_datapoint_count})"
|
||||
)
|
||||
)
|
||||
stop_collection_requested.set()
|
||||
return
|
||||
|
||||
if not has_consumed_any_datapoint and (now - collection_loop_start) > cycle_start_guard_seconds:
|
||||
if not has_consumed_any_datapoint and (now - loop_start_timestamp) > cycle_start_guard_seconds:
|
||||
collection_errors.append(
|
||||
TimeoutError(
|
||||
f"Device {device_connection.serial_number} streamed datapoints but never "
|
||||
@@ -114,6 +132,22 @@ def collect_complete_running_sweep_cycles(
|
||||
stop_collection_requested.set()
|
||||
return
|
||||
|
||||
# Hard wallclock deadline for the entire cycle. Even if every
|
||||
# datapoint refreshes `last_consumed_timestamp` and the per-packet
|
||||
# timeout never trips, we still bail out once the cycle has dragged
|
||||
# on for too long — this is the safety net the per-device timeout
|
||||
# cannot provide by itself.
|
||||
if (now - loop_start_timestamp) > _MAX_FULL_CYCLE_SECONDS:
|
||||
collection_errors.append(
|
||||
TimeoutError(
|
||||
f"Device {device_connection.serial_number} did not finish a sweep cycle "
|
||||
f"within {_MAX_FULL_CYCLE_SECONDS:.1f} s "
|
||||
f"(received {datapoints_received}/{expected_datapoint_count})"
|
||||
)
|
||||
)
|
||||
stop_collection_requested.set()
|
||||
return
|
||||
|
||||
try:
|
||||
packet_type, payload = device_connection.receive_packet(
|
||||
timeout_seconds=min(1.0, remaining_timeout_seconds)
|
||||
@@ -136,9 +170,11 @@ def collect_complete_running_sweep_cycles(
|
||||
|
||||
parsed_datapoint = parse_vna_datapoint_payload(payload)
|
||||
if parsed_datapoint and 0 <= parsed_datapoint.point_index < point_count:
|
||||
last_datapoint_timestamp = time.monotonic()
|
||||
datapoint_was_consumed = handle_datapoint(parsed_datapoint)
|
||||
if datapoint_was_consumed:
|
||||
# Only refreshed on accepted datapoints so the no-progress
|
||||
# timeout above stays honest about real cycle progress.
|
||||
last_consumed_timestamp = time.monotonic()
|
||||
has_consumed_any_datapoint = True
|
||||
datapoint_counts_by_device_serial[device_connection.serial_number] += 1
|
||||
datapoints_received += 1
|
||||
@@ -269,8 +305,35 @@ def collect_complete_running_sweep_cycles(
|
||||
|
||||
for collection_thread in collection_threads:
|
||||
collection_thread.start()
|
||||
|
||||
# Bounded join. Threads self-poll `stop_collection_requested` at most every
|
||||
# ~0.2 s (the queue.get timeout inside `receive_packet`), so a 2 s grace
|
||||
# period is more than enough for a cooperative shutdown. Anything still
|
||||
# alive after that is treated as an orphan: we set the flag a second time,
|
||||
# record an error so callers go through recovery, and stop waiting. The
|
||||
# thread is a daemon and will die with the producer process.
|
||||
deadline = time.monotonic() + _THREAD_JOIN_TIMEOUT_SECONDS
|
||||
for collection_thread in collection_threads:
|
||||
collection_thread.join()
|
||||
remaining_seconds = deadline - time.monotonic()
|
||||
collection_thread.join(timeout=max(0.0, remaining_seconds))
|
||||
|
||||
stalled_threads = [
|
||||
collection_thread for collection_thread in collection_threads if collection_thread.is_alive()
|
||||
]
|
||||
if stalled_threads:
|
||||
stop_collection_requested.set()
|
||||
# Give them one more short window in case they were just slow to react.
|
||||
secondary_deadline = time.monotonic() + 0.5
|
||||
for stalled_thread in stalled_threads:
|
||||
stalled_thread.join(timeout=max(0.0, secondary_deadline - time.monotonic()))
|
||||
still_stalled = [stalled_thread for stalled_thread in stalled_threads if stalled_thread.is_alive()]
|
||||
if still_stalled:
|
||||
collection_errors.append(
|
||||
RuntimeError(
|
||||
"Sweep collector thread(s) failed to stop within the join deadline: "
|
||||
+ ", ".join(stalled_thread.name for stalled_thread in still_stalled)
|
||||
)
|
||||
)
|
||||
|
||||
if collection_errors:
|
||||
raise RuntimeError(f"Sweep collection failed: {collection_errors[0]}") from collection_errors[0]
|
||||
|
||||
@@ -20,6 +20,12 @@ if TYPE_CHECKING:
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Delays applied between successive USB reopen attempts inside recover(). Picked
|
||||
# to give libusb time to re-enumerate a stuck device while staying short enough
|
||||
# that a healthy reconnect feels instant. The total worst-case wait is the sum
|
||||
# of all entries (1.75 s today) plus the cost of close()/open() themselves.
|
||||
_REOPEN_BACKOFF_SECONDS: tuple[float, ...] = (0.25, 0.5, 1.0)
|
||||
|
||||
_INPUT_S_PARAMETERS_BY_OUTPUT: dict[int, tuple[str, ...]] = {
|
||||
0: ("s31", "s41", "s51", "s61"),
|
||||
1: ("s32", "s42", "s52", "s62"),
|
||||
@@ -77,18 +83,60 @@ class MultiDeviceLibreVnaService:
|
||||
self._controller = None
|
||||
|
||||
def close(self) -> None:
|
||||
"""Close native device transports."""
|
||||
if self._controller is not None:
|
||||
self._controller.close()
|
||||
"""Close native device transports; never raises.
|
||||
|
||||
Recovery loops rely on `close()` being safe to call on a half-open or
|
||||
already-broken controller. We swallow any transport-level exception here
|
||||
and just drop the reference so the next `open()` starts fresh.
|
||||
"""
|
||||
controller = self._controller
|
||||
self._controller = None
|
||||
if controller is None:
|
||||
return
|
||||
try:
|
||||
controller.close()
|
||||
except Exception as exc: # noqa: BLE001 — recovery path, never propagate
|
||||
logger.warning("Multi-device close() ignored transport error: %s", exc)
|
||||
|
||||
def recover(self) -> None:
|
||||
"""Reopen native device transports after a failed acquisition."""
|
||||
"""Reopen native device transports after a failed acquisition.
|
||||
|
||||
Tries several short backoffs so a transient USB stall does not kill the
|
||||
producer on the very first retry. Raises the last error only after
|
||||
every attempt failed — the outer acquisition loop is expected to count
|
||||
these as recovery_attempts.
|
||||
"""
|
||||
if self._using_mock_backend:
|
||||
return
|
||||
self.close()
|
||||
time.sleep(0.25)
|
||||
|
||||
last_error: Exception | None = None
|
||||
for attempt_index, delay_s in enumerate(_REOPEN_BACKOFF_SECONDS, start=1):
|
||||
time.sleep(delay_s)
|
||||
try:
|
||||
self.open()
|
||||
if self._controller is not None:
|
||||
logger.info(
|
||||
"Multi-device reopen succeeded on attempt %d/%d (after %.2fs)",
|
||||
attempt_index,
|
||||
len(_REOPEN_BACKOFF_SECONDS),
|
||||
delay_s,
|
||||
)
|
||||
return
|
||||
except Exception as exc: # noqa: BLE001 — propagate only the last failure
|
||||
last_error = exc
|
||||
logger.warning(
|
||||
"Multi-device reopen attempt %d/%d failed after %.2fs: %s",
|
||||
attempt_index,
|
||||
len(_REOPEN_BACKOFF_SECONDS),
|
||||
delay_s,
|
||||
exc,
|
||||
)
|
||||
self.close() # tidy partially-opened state before next try
|
||||
|
||||
if last_error is not None:
|
||||
raise last_error
|
||||
raise RuntimeError("Multi-device recover() exhausted all reopen attempts")
|
||||
|
||||
def configure(self, sweep: RadarSweepModel) -> None:
|
||||
"""Store sweep settings for subsequent full-matrix acquisitions."""
|
||||
@@ -132,7 +180,21 @@ class MultiDeviceLibreVnaService:
|
||||
exc,
|
||||
exc_info=True,
|
||||
)
|
||||
# recover() may itself fail when libusb cannot re-enumerate the
|
||||
# device fast enough; treat that as the same kind of recovery
|
||||
# attempt and try again on the next loop iteration, so a
|
||||
# transient USB hiccup cannot kill the whole producer.
|
||||
try:
|
||||
self.recover()
|
||||
except Exception as recover_exc: # noqa: BLE001
|
||||
last_error = recover_exc
|
||||
logger.warning(
|
||||
"multi-device recover() failed (%d/%d): %s",
|
||||
attempt_index + 1,
|
||||
self.recovery_attempts,
|
||||
recover_exc,
|
||||
exc_info=True,
|
||||
)
|
||||
|
||||
assert last_error is not None
|
||||
raise last_error
|
||||
|
||||
@@ -10,12 +10,15 @@ synchronized SCPI round trip.
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import logging
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
import pyvisa
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
from python_app.models.dataset_model import ComboKey, SweepCollection, TraceData
|
||||
from python_app.models.run_config_model import RadarSweepModel
|
||||
|
||||
@@ -324,7 +327,12 @@ class Sn9000Service:
|
||||
try:
|
||||
instrument.read_bytes(1, break_on_termchar=True)
|
||||
return
|
||||
except Exception:
|
||||
except pyvisa.errors.VisaIOError as exc:
|
||||
# Timeouts on a trailing newline are routine; anything else
|
||||
# likely means HiSLIP framing is out of sync and the next
|
||||
# request will hang — surface it in the logs.
|
||||
if exc.error_code != pyvisa.constants.StatusCode.error_timeout:
|
||||
logger.warning("SN9000 terminator drain failed: %s", exc)
|
||||
return
|
||||
|
||||
def _read_response_bytes(self, count: int) -> bytes:
|
||||
|
||||
@@ -377,6 +377,14 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel:
|
||||
gui.processing.legacy_gpr.ignore_socket_speed_enabled,
|
||||
),
|
||||
look_angle_deg=legacy_float("look_angle_deg", gui.processing.legacy_gpr.look_angle_deg),
|
||||
apply_freq_phase_correction=legacy_bool(
|
||||
"apply_freq_phase_correction",
|
||||
gui.processing.legacy_gpr.apply_freq_phase_correction,
|
||||
),
|
||||
reference_mode=legacy_string(
|
||||
"reference_mode",
|
||||
gui.processing.legacy_gpr.reference_mode,
|
||||
),
|
||||
snr_thresh=legacy_float("snr_thresh", gui.processing.legacy_gpr.snr_thresh),
|
||||
snr_comp_max=legacy_float("snr_comp_max", gui.processing.legacy_gpr.snr_comp_max),
|
||||
background_subtract_enabled=legacy_bool(
|
||||
@@ -410,6 +418,10 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel:
|
||||
raise ValueError("gui.processing.legacy_gpr.mode must be one of: point, extended")
|
||||
if gui.processing.legacy_gpr.render_mode not in {"heatmap", "objects_only"}:
|
||||
raise ValueError("gui.processing.legacy_gpr.render_mode must be one of: heatmap, objects_only")
|
||||
if gui.processing.legacy_gpr.reference_mode not in {"frame_center", "first_tx_event"}:
|
||||
raise ValueError(
|
||||
"gui.processing.legacy_gpr.reference_mode must be one of: frame_center, first_tx_event"
|
||||
)
|
||||
if gui.processing.gpr.range_comp_power < 0.0:
|
||||
raise ValueError("gui.processing.gpr.range_comp_power must be >= 0")
|
||||
if gui.processing.gpr.angle_comp_power < 0.0:
|
||||
@@ -552,6 +564,8 @@ def gui_profile_to_dict(model: GuiProfileModel) -> dict[str, Any]:
|
||||
"speed_m_s": gui.processing.legacy_gpr.speed_m_s,
|
||||
"ignore_socket_speed_enabled": gui.processing.legacy_gpr.ignore_socket_speed_enabled,
|
||||
"look_angle_deg": gui.processing.legacy_gpr.look_angle_deg,
|
||||
"apply_freq_phase_correction": gui.processing.legacy_gpr.apply_freq_phase_correction,
|
||||
"reference_mode": gui.processing.legacy_gpr.reference_mode,
|
||||
"snr_thresh": gui.processing.legacy_gpr.snr_thresh,
|
||||
"snr_comp_max": gui.processing.legacy_gpr.snr_comp_max,
|
||||
"background_subtract_enabled": gui.processing.legacy_gpr.background_subtract_enabled,
|
||||
|
||||
@@ -88,6 +88,8 @@ class GuiLegacyGprStateModel:
|
||||
speed_m_s: float = 0.0
|
||||
ignore_socket_speed_enabled: bool = False
|
||||
look_angle_deg: float = 0.0
|
||||
apply_freq_phase_correction: bool = True
|
||||
reference_mode: str = "frame_center"
|
||||
snr_thresh: float = 4.5
|
||||
snr_comp_max: float = 25.0
|
||||
background_subtract_enabled: bool = True
|
||||
|
||||
@@ -24,10 +24,22 @@ def _as_dict(value: Any, context: str) -> dict[str, Any]:
|
||||
return value
|
||||
|
||||
|
||||
def _read_str(payload: dict[str, Any], key: str, default: str) -> str:
|
||||
"""Return payload string, treating an explicit JSON `null` as missing.
|
||||
|
||||
`payload.get(key, default)` returns `None` when the key exists with value
|
||||
`null`, which is then coerced into the literal string `"None"` by `str()`.
|
||||
"""
|
||||
value = payload.get(key, default)
|
||||
if value is None:
|
||||
return default
|
||||
return str(value)
|
||||
|
||||
|
||||
def _load_preprocess_asset(payload: dict[str, Any], target: PreprocessAssetModel) -> None:
|
||||
"""Load preprocess asset fields into target model."""
|
||||
target.set_name = str(payload.get("set_name", target.set_name))
|
||||
target.bundle_path = str(payload.get("bundle_path", target.bundle_path))
|
||||
target.set_name = _read_str(payload, "set_name", target.set_name)
|
||||
target.bundle_path = _read_str(payload, "bundle_path", target.bundle_path)
|
||||
|
||||
|
||||
def _load_string_list(payload: dict[str, Any], key: str, context: str) -> list[str]:
|
||||
|
||||
@@ -4,8 +4,11 @@ from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class GuiSessionState:
|
||||
@@ -32,13 +35,22 @@ class GuiSessionStateStore:
|
||||
if not self._path.exists():
|
||||
return GuiSessionState()
|
||||
|
||||
try:
|
||||
payload = json.loads(self._path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as exc:
|
||||
# The state file is GUI-local cache — a corrupted file should not
|
||||
# prevent the app from starting. Reset to defaults and let the
|
||||
# next write overwrite it.
|
||||
logger.warning("Resetting unreadable GUI session-state %s: %s", self._path, exc)
|
||||
return GuiSessionState()
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"GUI session-state root must be JSON object: {self._path}")
|
||||
logger.warning("Resetting GUI session-state with non-object root: %s", self._path)
|
||||
return GuiSessionState()
|
||||
|
||||
raw_path = payload.get("last_profile_path", "")
|
||||
if not isinstance(raw_path, str):
|
||||
raise ValueError("GUI session-state `last_profile_path` must be a string")
|
||||
logger.warning("Resetting GUI session-state with non-string last_profile_path: %s", self._path)
|
||||
return GuiSessionState()
|
||||
return GuiSessionState(last_profile_path=raw_path)
|
||||
|
||||
def write(self, state: GuiSessionState) -> Path:
|
||||
|
||||
@@ -36,6 +36,16 @@ class ProcessingLiveConfig:
|
||||
gpr_draw_top_m_objects: int = 2
|
||||
gpr_speed_m_s: float = 0.0
|
||||
gpr_look_angle_deg: float = 0.0
|
||||
# Motion-model knobs for the legacy GPR pipeline. `direction_sign` flips
|
||||
# which way later events appear deeper (+1) vs shallower (-1) along Z.
|
||||
# `apply_freq_phase_correction` enables intra-sweep frequency-domain phase
|
||||
# compensation that fires *before* the IFFT — needed when the radar moves
|
||||
# appreciably during one sweep.
|
||||
gpr_direction_sign: float = 1.0
|
||||
gpr_apply_freq_phase_correction: bool = True
|
||||
# Anchor for the motion model's per-event `dt_ref`: 'frame_center' (default)
|
||||
# or 'first_tx_event'. Mirrors Python `MOTION_CONFIG.reference_mode`.
|
||||
gpr_reference_mode: str = "frame_center"
|
||||
gpr_snr_thresh: float = 4.5
|
||||
gpr_snr_comp_max: float = 25.0
|
||||
gpr_start_freq_mhz: float = 3000.0
|
||||
@@ -93,6 +103,9 @@ class ProcessingLiveConfig:
|
||||
"gpr_draw_top_m_objects": int(self.gpr_draw_top_m_objects),
|
||||
"gpr_speed_m_s": float(self.gpr_speed_m_s),
|
||||
"gpr_look_angle_deg": float(self.gpr_look_angle_deg),
|
||||
"gpr_direction_sign": float(self.gpr_direction_sign),
|
||||
"gpr_apply_freq_phase_correction": bool(self.gpr_apply_freq_phase_correction),
|
||||
"gpr_reference_mode": str(self.gpr_reference_mode),
|
||||
"gpr_snr_thresh": float(self.gpr_snr_thresh),
|
||||
"gpr_snr_comp_max": float(self.gpr_snr_comp_max),
|
||||
"gpr_start_freq_mhz": float(self.gpr_start_freq_mhz),
|
||||
|
||||
@@ -4,18 +4,41 @@ from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from contextlib import suppress
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import threading
|
||||
import time
|
||||
|
||||
from python_app.hardware_full.matrix_radar_service import create_matrix_radar_service
|
||||
from python_app.hardware_full.matrix_radar_service import MatrixRadarService, create_matrix_radar_service
|
||||
from python_app.models.run_config_model import RunConfigModel
|
||||
from python_app.orchestration.shm import ShmRingWriter
|
||||
from python_app.storage.npz.serialize import RAW_MAGIC, serialize_trace_collection
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Maximum number of acquisitions allowed to fail in a row before we give up and
|
||||
# let the supervisor restart the whole process. Picked high enough to survive
|
||||
# transient USB stalls (each retry triggers a full reset cycle of ~1-2s) but
|
||||
# bounded so a permanently broken device does not loop forever.
|
||||
_MAX_CONSECUTIVE_ACQUIRE_FAILURES = 20
|
||||
# Cooldown applied between a failed acquire and the next reset attempt. Stops
|
||||
# us from busy-spinning when the device keeps refusing to come back.
|
||||
_ACQUIRE_FAILURE_COOLDOWN_S = 1.0
|
||||
|
||||
|
||||
def _reset_radar_service(
|
||||
config: RunConfigModel, previous: MatrixRadarService | None
|
||||
) -> MatrixRadarService:
|
||||
"""Close `previous` (best-effort) and return a freshly opened+configured service."""
|
||||
if previous is not None:
|
||||
with suppress(Exception):
|
||||
previous.close()
|
||||
radar = create_matrix_radar_service(config)
|
||||
radar.open()
|
||||
radar.configure(config.radar.sweep)
|
||||
return radar
|
||||
|
||||
|
||||
def main() -> int:
|
||||
"""Run producer process until config or signal requests exit."""
|
||||
@@ -50,15 +73,53 @@ def main() -> int:
|
||||
config.rings.raw_tap.capacity,
|
||||
config.rings.raw_tap.slot_size_bytes,
|
||||
)
|
||||
radar = create_matrix_radar_service(config)
|
||||
|
||||
radar: MatrixRadarService | None = None
|
||||
consecutive_failures = 0
|
||||
try:
|
||||
radar.open()
|
||||
radar.configure(config.radar.sweep)
|
||||
radar = _reset_radar_service(config, previous=None)
|
||||
collection_id = 1
|
||||
while not stop_requested.is_set():
|
||||
collection_start = time.monotonic()
|
||||
try:
|
||||
if radar is None:
|
||||
radar = _reset_radar_service(config, previous=None)
|
||||
collection = radar.acquire_collection(collection_id=collection_id)
|
||||
except Exception as exc: # noqa: BLE001 — top-level recovery is the point
|
||||
consecutive_failures += 1
|
||||
if consecutive_failures > _MAX_CONSECUTIVE_ACQUIRE_FAILURES:
|
||||
logger.error(
|
||||
"Matrix radar acquisition failed %d times in a row; giving up. "
|
||||
"Last error: %s",
|
||||
consecutive_failures - 1,
|
||||
exc,
|
||||
)
|
||||
raise
|
||||
logger.warning(
|
||||
"Matrix radar acquisition failed (%d/%d), resetting service: %s",
|
||||
consecutive_failures,
|
||||
_MAX_CONSECUTIVE_ACQUIRE_FAILURES,
|
||||
exc,
|
||||
exc_info=True,
|
||||
)
|
||||
# Cooldown gives slow USB stacks (and the device firmware) time
|
||||
# to settle before the next open() attempt.
|
||||
if stop_requested.wait(_ACQUIRE_FAILURE_COOLDOWN_S):
|
||||
break
|
||||
try:
|
||||
radar = _reset_radar_service(config, previous=radar)
|
||||
except Exception as reset_exc: # noqa: BLE001
|
||||
logger.warning(
|
||||
"Matrix radar reset (%d/%d) failed, will retry: %s",
|
||||
consecutive_failures,
|
||||
_MAX_CONSECUTIVE_ACQUIRE_FAILURES,
|
||||
reset_exc,
|
||||
exc_info=True,
|
||||
)
|
||||
radar = None
|
||||
continue
|
||||
|
||||
consecutive_failures = 0
|
||||
|
||||
payload = serialize_trace_collection(collection, RAW_MAGIC)
|
||||
if not raw_writer.push(payload):
|
||||
@@ -80,6 +141,8 @@ def main() -> int:
|
||||
)
|
||||
collection_id += 1
|
||||
finally:
|
||||
if radar is not None:
|
||||
with suppress(Exception):
|
||||
radar.close()
|
||||
raw_tap_writer.close()
|
||||
raw_writer.close()
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from contextlib import suppress
|
||||
from datetime import datetime
|
||||
import json
|
||||
from pathlib import Path
|
||||
@@ -65,7 +66,6 @@ class NpzStore(StoreApi):
|
||||
}
|
||||
)
|
||||
|
||||
np.savez(npz_path, **payload)
|
||||
meta = {
|
||||
"collection_id": int(collection.collection_id),
|
||||
"monotonic_ns": int(collection.monotonic_ns),
|
||||
@@ -73,7 +73,22 @@ class NpzStore(StoreApi):
|
||||
"capture_end_ns": int(collection.capture_end_ns),
|
||||
"combos": combo_records,
|
||||
}
|
||||
meta_path.write_text(json.dumps(meta, indent=2), encoding="utf-8")
|
||||
|
||||
# Write both files to temporary paths first, then atomically rename so a
|
||||
# crash never leaves an .npz without its meta (or vice versa).
|
||||
npz_tmp = npz_path.with_name(npz_path.name + ".tmp")
|
||||
meta_tmp = meta_path.with_name(meta_path.name + ".tmp")
|
||||
try:
|
||||
with npz_tmp.open("wb") as npz_file:
|
||||
np.savez(npz_file, **payload)
|
||||
meta_tmp.write_text(json.dumps(meta, indent=2), encoding="utf-8")
|
||||
npz_tmp.replace(npz_path)
|
||||
meta_tmp.replace(meta_path)
|
||||
except BaseException:
|
||||
for tmp_path in (npz_tmp, meta_tmp):
|
||||
with suppress(OSError):
|
||||
tmp_path.unlink(missing_ok=True)
|
||||
raise
|
||||
|
||||
def load_set(self, kind: str, radar_key: str, set_name: str) -> SweepCollection:
|
||||
"""Load named preprocess set from NPZ representation."""
|
||||
@@ -84,8 +99,10 @@ class NpzStore(StoreApi):
|
||||
if not npz_path.exists() or not meta_path.exists():
|
||||
raise FileNotFoundError(f"Missing set files for {kind}/{radar_key}/{set_name}")
|
||||
|
||||
set_label = f"{kind}/{radar_key}/{set_name}"
|
||||
try:
|
||||
meta = json.loads(meta_path.read_text(encoding="utf-8"))
|
||||
arrays = np.load(npz_path)
|
||||
arrays = np.load(npz_path, allow_pickle=False)
|
||||
|
||||
traces: list[TraceData] = []
|
||||
for combo in meta["combos"]:
|
||||
@@ -108,6 +125,8 @@ class NpzStore(StoreApi):
|
||||
capture_start_ns=int(meta.get("capture_start_ns", 0)),
|
||||
capture_end_ns=int(meta.get("capture_end_ns", 0)),
|
||||
)
|
||||
except (json.JSONDecodeError, KeyError, TypeError, ValueError) as exc:
|
||||
raise RuntimeError(f"Corrupted preprocess set {set_label}: {exc}") from exc
|
||||
|
||||
def list_sets(self, kind: str, radar_key: str) -> list[str]:
|
||||
"""List available set names for `(kind, radar_key)`."""
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
"""Tests for Kamil ADC config, parser, and producer wiring."""
|
||||
"""Tests for Kamil ADC config, frame parsing, TTY reader, and producer wiring."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -9,10 +9,14 @@ import pty
|
||||
import struct
|
||||
import sys
|
||||
import tempfile
|
||||
import time
|
||||
import tty
|
||||
import unittest
|
||||
|
||||
from python_app.hardware_full.kamil_adc_service import KamilAdcFrameParser, KamilAdcTtyReader
|
||||
from python_app.hardware_full.kamil_adc_service import (
|
||||
KamilAdcTtyReader,
|
||||
_parse_point_frame,
|
||||
)
|
||||
from python_app.models.run_config_model import RunConfigModel
|
||||
from python_app.orchestration.process_supervisor import ProcessSupervisor
|
||||
|
||||
@@ -25,28 +29,41 @@ def _point_frame(step: int, real: int, imag: int, *, marker: int = 0x000A) -> by
|
||||
return struct.pack("<HHhh", marker, step, real, imag)
|
||||
|
||||
|
||||
class KamilAdcFrameParserTest(unittest.TestCase):
|
||||
def test_parse_valid_point(self) -> None:
|
||||
value = KamilAdcFrameParser.parse_point(_point_frame(1, 123, -45), expected_step=1)
|
||||
class ParsePointFrameTest(unittest.TestCase):
|
||||
def test_parses_valid_point(self) -> None:
|
||||
value = _parse_point_frame(_point_frame(1, 123, -45), expected_step=1)
|
||||
self.assertEqual(value, complex(123, -45))
|
||||
|
||||
def test_bad_marker_is_rejected(self) -> None:
|
||||
def test_rejects_bad_marker(self) -> None:
|
||||
with self.assertRaisesRegex(ValueError, "marker mismatch"):
|
||||
KamilAdcFrameParser.parse_point(_point_frame(1, 10, 20, marker=0x001A), expected_step=1)
|
||||
_parse_point_frame(_point_frame(1, 10, 20, marker=0x001A), expected_step=1)
|
||||
|
||||
def test_wrong_step_is_rejected(self) -> None:
|
||||
def test_rejects_wrong_step(self) -> None:
|
||||
with self.assertRaisesRegex(ValueError, "step mismatch"):
|
||||
KamilAdcFrameParser.parse_point(_point_frame(2, 10, 20), expected_step=1)
|
||||
_parse_point_frame(_point_frame(2, 10, 20), expected_step=1)
|
||||
|
||||
|
||||
class KamilAdcTtyReaderTest(unittest.TestCase):
|
||||
def test_valid_stream_reads_complex_sweep(self) -> None:
|
||||
"""End-to-end tests over a PTY exercising the background reader thread."""
|
||||
|
||||
def _open_pty_reader(self) -> tuple[int, int, KamilAdcTtyReader]:
|
||||
master_fd, slave_fd = pty.openpty()
|
||||
reader: KamilAdcTtyReader | None = None
|
||||
try:
|
||||
tty.setraw(slave_fd)
|
||||
reader = KamilAdcTtyReader(os.ttyname(slave_fd))
|
||||
reader.open()
|
||||
return master_fd, slave_fd, reader
|
||||
|
||||
@staticmethod
|
||||
def _close(master_fd: int, slave_fd: int, reader: KamilAdcTtyReader) -> None:
|
||||
try:
|
||||
reader.close()
|
||||
finally:
|
||||
os.close(master_fd)
|
||||
os.close(slave_fd)
|
||||
|
||||
def test_publishes_first_complete_sweep(self) -> None:
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
os.write(
|
||||
master_fd,
|
||||
_start_frame()
|
||||
@@ -54,23 +71,39 @@ class KamilAdcTtyReaderTest(unittest.TestCase):
|
||||
+ _point_frame(2, -20, 2)
|
||||
+ _start_frame(),
|
||||
)
|
||||
|
||||
values = reader.read_sweep(timeout_s=1.0)
|
||||
|
||||
self.assertEqual(values.tolist(), [complex(10, -1), complex(-20, 2)])
|
||||
self.assertEqual(reader.locked_points, 2)
|
||||
finally:
|
||||
if reader is not None:
|
||||
reader.close()
|
||||
os.close(master_fd)
|
||||
os.close(slave_fd)
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
def test_stream_reads_consecutive_variable_length_sweeps(self) -> None:
|
||||
master_fd, slave_fd = pty.openpty()
|
||||
reader: KamilAdcTtyReader | None = None
|
||||
def test_consecutive_constant_length_sweeps(self) -> None:
|
||||
"""Each newly-completed sweep is delivered once new data arrives after a read."""
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
os.write(
|
||||
master_fd,
|
||||
_start_frame()
|
||||
+ _point_frame(1, 10, -1)
|
||||
+ _point_frame(2, -20, 2)
|
||||
+ _start_frame(),
|
||||
)
|
||||
first = reader.read_sweep(timeout_s=1.0)
|
||||
self.assertEqual(first.tolist(), [complex(10, -1), complex(-20, 2)])
|
||||
|
||||
os.write(
|
||||
master_fd,
|
||||
_point_frame(1, 30, -3) + _point_frame(2, -40, 4) + _start_frame(),
|
||||
)
|
||||
second = reader.read_sweep(timeout_s=1.0)
|
||||
self.assertEqual(second.tolist(), [complex(30, -3), complex(-40, 4)])
|
||||
finally:
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
def test_shorter_sweep_after_lock_raises(self) -> None:
|
||||
"""A later sweep with fewer points than the locked-in count fails fast."""
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
tty.setraw(slave_fd)
|
||||
reader = KamilAdcTtyReader(os.ttyname(slave_fd))
|
||||
reader.open()
|
||||
os.write(
|
||||
master_fd,
|
||||
_start_frame()
|
||||
@@ -80,60 +113,73 @@ class KamilAdcTtyReaderTest(unittest.TestCase):
|
||||
+ _point_frame(1, 30, -3)
|
||||
+ _start_frame(),
|
||||
)
|
||||
|
||||
first = reader.read_sweep(timeout_s=1.0)
|
||||
second = reader.read_sweep(timeout_s=1.0)
|
||||
|
||||
self.assertEqual(first.tolist(), [complex(10, -1), complex(-20, 2)])
|
||||
self.assertEqual(second.tolist(), [complex(30, -3)])
|
||||
with self.assertRaisesRegex(RuntimeError, "sweep length changed"):
|
||||
reader.read_sweep(timeout_s=1.0)
|
||||
finally:
|
||||
if reader is not None:
|
||||
reader.close()
|
||||
os.close(master_fd)
|
||||
os.close(slave_fd)
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
def test_expected_point_count_discards_mismatched_sweep(self) -> None:
|
||||
master_fd, slave_fd = pty.openpty()
|
||||
reader: KamilAdcTtyReader | None = None
|
||||
def test_longer_sweep_after_lock_raises(self) -> None:
|
||||
"""A later sweep with more points than the locked-in count fails fast."""
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
tty.setraw(slave_fd)
|
||||
reader = KamilAdcTtyReader(os.ttyname(slave_fd))
|
||||
reader.open()
|
||||
os.write(
|
||||
master_fd,
|
||||
_start_frame()
|
||||
+ _point_frame(1, 5, -5)
|
||||
+ _start_frame()
|
||||
+ _point_frame(1, 10, -1)
|
||||
+ _point_frame(2, -20, 2)
|
||||
+ _start_frame()
|
||||
+ _point_frame(1, 30, -3)
|
||||
+ _point_frame(2, -40, 4)
|
||||
+ _start_frame(),
|
||||
)
|
||||
|
||||
values = reader.read_sweep(timeout_s=1.0, expected_points=2)
|
||||
|
||||
self.assertEqual(values.tolist(), [complex(10, -1), complex(-20, 2)])
|
||||
first = reader.read_sweep(timeout_s=1.0)
|
||||
self.assertEqual(first.tolist(), [complex(10, -1)])
|
||||
with self.assertRaisesRegex(RuntimeError, "exceeded locked point count"):
|
||||
reader.read_sweep(timeout_s=1.0)
|
||||
finally:
|
||||
if reader is not None:
|
||||
reader.close()
|
||||
os.close(master_fd)
|
||||
os.close(slave_fd)
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
def test_stream_without_next_start_times_out_with_received_count(self) -> None:
|
||||
master_fd, slave_fd = pty.openpty()
|
||||
reader: KamilAdcTtyReader | None = None
|
||||
def test_no_completed_sweep_times_out(self) -> None:
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
tty.setraw(slave_fd)
|
||||
reader = KamilAdcTtyReader(os.ttyname(slave_fd))
|
||||
reader.open()
|
||||
# Start marker plus a partial sweep with no follow-up boundary.
|
||||
os.write(master_fd, _start_frame() + _point_frame(1, 10, -1))
|
||||
|
||||
with self.assertRaisesRegex(TimeoutError, "sweep end: received 1 points"):
|
||||
reader.read_sweep(timeout_s=0.05)
|
||||
with self.assertRaisesRegex(TimeoutError, "Timed out waiting for Kamil ADC sweep"):
|
||||
reader.read_sweep(timeout_s=0.1)
|
||||
finally:
|
||||
if reader is not None:
|
||||
reader.close()
|
||||
os.close(master_fd)
|
||||
os.close(slave_fd)
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
def test_only_latest_sweep_is_published(self) -> None:
|
||||
"""If multiple sweeps arrive before the consumer reads, only the newest survives."""
|
||||
master_fd, slave_fd, reader = self._open_pty_reader()
|
||||
try:
|
||||
payload = (
|
||||
_start_frame()
|
||||
+ _point_frame(1, 1, 0)
|
||||
+ _point_frame(2, 2, 0)
|
||||
+ _start_frame()
|
||||
+ _point_frame(1, 3, 0)
|
||||
+ _point_frame(2, 4, 0)
|
||||
+ _start_frame()
|
||||
+ _point_frame(1, 5, 0)
|
||||
+ _point_frame(2, 6, 0)
|
||||
+ _start_frame()
|
||||
)
|
||||
os.write(master_fd, payload)
|
||||
# Wait until the reader thread has parsed all three sweeps before
|
||||
# reading from the mailbox — otherwise we'd race the producer and
|
||||
# might consume an intermediate value.
|
||||
deadline = time.monotonic() + 1.0
|
||||
while time.monotonic() < deadline and reader.published_count < 3:
|
||||
time.sleep(0.005)
|
||||
self.assertGreaterEqual(reader.published_count, 3)
|
||||
values = reader.read_sweep(timeout_s=1.0)
|
||||
# The reader thread overwrites unread sweeps; the consumer sees the
|
||||
# most recently completed one.
|
||||
self.assertEqual(values.tolist(), [complex(5, 0), complex(6, 0)])
|
||||
finally:
|
||||
self._close(master_fd, slave_fd, reader)
|
||||
|
||||
|
||||
class KamilAdcConfigTest(unittest.TestCase):
|
||||
|
||||
@@ -217,8 +217,8 @@ class MultiRadarSequentialCaptureSession:
|
||||
display_traces.append(combined_collection.traces[-1])
|
||||
variant_labels.append(variant.display_name)
|
||||
else:
|
||||
assert self._input_switch is not None
|
||||
assert self._output_switch is not None
|
||||
if self._input_switch is None or self._output_switch is None:
|
||||
raise RuntimeError("Switches are not initialised for combo capture")
|
||||
self._output_switch.switch_to(combo.output)
|
||||
self._input_switch.switch_to(combo.input)
|
||||
if self._base_config.runtime.settling_ms > 0:
|
||||
|
||||
@@ -179,8 +179,8 @@ class SequentialCaptureSession:
|
||||
self._next_index = len(self._combos)
|
||||
return combined_collection.traces[-1]
|
||||
|
||||
assert self._input_switch is not None
|
||||
assert self._output_switch is not None
|
||||
if self._input_switch is None or self._output_switch is None:
|
||||
raise RuntimeError("Switches are not initialised for combo capture")
|
||||
self._output_switch.switch_to(combo.output)
|
||||
self._input_switch.switch_to(combo.input)
|
||||
if self._config.runtime.settling_ms > 0:
|
||||
|
||||
Reference in New Issue
Block a user