diff --git a/Ellips_motion_remake_2.py b/Ellips_motion_remake_2.py new file mode 100644 index 0000000..22f39a6 --- /dev/null +++ b/Ellips_motion_remake_2.py @@ -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() \ No newline at end of file diff --git a/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp b/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp index 8b71f9e..74bd2fa 100644 --- a/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp +++ b/data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp @@ -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; diff --git a/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp b/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp index 84f34ad..a7b480b 100644 --- a/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp +++ b/data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp @@ -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(found->get()); } + 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(found->get()); + } + 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(); + } + 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(), "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"); diff --git a/data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp b/data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp index 7600b59..aa5b386 100644 --- a/data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp +++ b/data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp @@ -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 time_s{}; @@ -36,8 +71,26 @@ struct LegacyRegionRecord { std::vector 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 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& x_tx, - const std::vector& 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& 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>& 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& selected_traces, + bool matrix_radar +) -> std::pair, std::size_t> { + std::unordered_map event_index_by_pair{}; + event_index_by_pair.reserve(selected_traces.size()); + + if (matrix_radar) { + std::unordered_map 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> 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& 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 { - std::unordered_map timing_by_pair{}; - timing_by_pair.reserve(traces.size()); +) -> LegacyMotionTiming { + LegacyMotionTiming timing{}; + timing.speed_m_s = static_cast(live_config.gpr_speed_m_s); + timing.direction_sign = static_cast(live_config.gpr_direction_sign); + timing.cos_look_angle = std::cos((static_cast(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(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{}); + if (selected_traces.empty()) { + return timing; + } + + 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_by_pair; + return timing; } - if (total_combo_count == 0U) { - throw std::runtime_error("Legacy GPR requires at least one run combo"); - } 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(capture_end_ns - capture_start_ns) * 1e-9; - const double slot_duration_s = capture_span_s / static_cast(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(capture_end_ns - capture_start_ns) * 1e-9; + const double event_duration_s = total_span_s / static_cast(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(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(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(live_config.gpr_look_angle_deg) * kPi) / 180.0); - for (const auto& trace : traces) { - const double t_center_s = (static_cast(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(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(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>& peaks_by_pair, - const std::unordered_map& 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>& exclude_ranges, double velocity_mps, double shell_sigma_m, + double min_depth_m, + double max_depth_m, const std::vector& x_tx, const std::vector& 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& 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& x_rx, double velocity_mps, double shell_sigma_m, + double min_depth_m, + double max_depth_m, LegacyPeakDomain domain ) -> std::pair, std::vector> { std::vector 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::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6)))); + static_cast(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6)))); const auto radius_z = - static_cast(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6)))); + static_cast(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6)))); std::vector> 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 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& x_tx, const std::vector& 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> { std::vector 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(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(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 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(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( diff --git a/data_acq_and_processing/sweep_orchestrator/device_drivers/interfaces/radar_driver.hpp b/data_acq_and_processing/sweep_orchestrator/device_drivers/interfaces/radar_driver.hpp index d1312fb..059da61 100644 --- a/data_acq_and_processing/sweep_orchestrator/device_drivers/interfaces/radar_driver.hpp +++ b/data_acq_and_processing/sweep_orchestrator/device_drivers/interfaces/radar_driver.hpp @@ -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 diff --git a/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp b/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp index 4cae316..d68b3f1 100644 --- a/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp +++ b/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp @@ -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(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(active_combo_.input_pos); + const auto output_pos = static_cast(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 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 contribution = std::polar( - 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; diff --git a/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna_minimal_driver.hpp b/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna_minimal_driver.hpp index 9c08064..633b137 100644 --- a/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna_minimal_driver.hpp +++ b/data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna_minimal_driver.hpp @@ -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; diff --git a/data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp b/data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp index d30fddf..1a75aa1 100644 --- a/data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp +++ b/data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp @@ -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); diff --git a/python_app/gui/app_window.py b/python_app/gui/app_window.py index 8e436f7..71010c1 100644 --- a/python_app/gui/app_window.py +++ b/python_app/gui/app_window.py @@ -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.""" diff --git a/python_app/gui/controllers/app_window_config/live_processing_mixin.py b/python_app/gui/controllers/app_window_config/live_processing_mixin.py index 6651f80..71d1e34 100644 --- a/python_app/gui/controllers/app_window_config/live_processing_mixin.py +++ b/python_app/gui/controllers/app_window_config/live_processing_mixin.py @@ -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, diff --git a/python_app/gui/controllers/app_window_config/profile_io_mixin.py b/python_app/gui/controllers/app_window_config/profile_io_mixin.py index afb5b04..139f7e7 100644 --- a/python_app/gui/controllers/app_window_config/profile_io_mixin.py +++ b/python_app/gui/controllers/app_window_config/profile_io_mixin.py @@ -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) ) diff --git a/python_app/gui/controllers/app_window_config/state_builders.py b/python_app/gui/controllers/app_window_config/state_builders.py index 259256f..6b670af 100644 --- a/python_app/gui/controllers/app_window_config/state_builders.py +++ b/python_app/gui/controllers/app_window_config/state_builders.py @@ -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()), diff --git a/python_app/gui/controllers/app_window_pipeline_mixin.py b/python_app/gui/controllers/app_window_pipeline_mixin.py index 18622d9..208732c 100644 --- a/python_app/gui/controllers/app_window_pipeline_mixin.py +++ b/python_app/gui/controllers/app_window_pipeline_mixin.py @@ -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() diff --git a/python_app/gui/controllers/sections/processing_section.py b/python_app/gui/controllers/sections/processing_section.py index bc859a8..232b6c9 100644 --- a/python_app/gui/controllers/sections/processing_section.py +++ b/python_app/gui/controllers/sections/processing_section.py @@ -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) diff --git a/python_app/hardware_full/kamil_adc_service.py b/python_app/hardware_full/kamil_adc_service.py index d0bc151..5c9d01a 100644 --- a/python_app/hardware_full/kamil_adc_service.py +++ b/python_app/hardware_full/kamil_adc_service.py @@ -1,4 +1,22 @@ -"""Service for acquiring sweeps from the external Kamil ADC collector.""" +"""Service for acquiring sweeps from the external Kamil ADC collector. + +The external `kamil_adc` binary publishes its samples on a PTY/TTY device as a +stream of 8-byte frames: + +* **Start marker**: `0x000A 0xFFFF 0xFFFF 0xFFFF` — delimits sweep boundaries. +* **Point frame**: `0x000A step real_i16 imag_i16` — one complex sample per + frame, with `step` running 1, 2, …, N for an N-point sweep. + +The hardware emits sweeps continuously, faster than callers tend to invoke +:meth:`KamilAdcService.acquire`. To avoid TTY-buffer overruns and stale data, +a daemon thread drains the device end of the TTY non-stop, parses complete +sweeps as they arrive, and publishes the **latest** one to a one-slot mailbox. +:meth:`acquire` simply waits for the next sweep to appear in that mailbox. + +Sweep length is determined by the first sweep observed at runtime and stays +constant for the life of the service; any later mismatch is treated as a +protocol violation rather than something to silently discard. +""" from __future__ import annotations @@ -13,6 +31,7 @@ import signal import stat import struct import subprocess +import threading import time import numpy as np @@ -22,233 +41,255 @@ from python_app.models.run_config_model import RadarSweepModel, RunConfigModel logger = logging.getLogger(__name__) +# Wire-format constants for the Kamil ADC TTY protocol. KAMIL_ADC_MARKER = 0x000A KAMIL_ADC_START_STEP = 0xFFFF KAMIL_ADC_FRAME_BYTES = 8 -KAMIL_ADC_MAX_STEP = 0xFFFE -_RAW_FRAME_STRUCT = struct.Struct(" bool: - """Return whether `frame` is the packet-start marker.""" - return frame == _START_FRAME - - @staticmethod - def parse_point(frame: bytes, expected_step: int) -> complex: - """Parse one `0x000A step real imag` frame and validate ordering.""" - if len(frame) != KAMIL_ADC_FRAME_BYTES: - raise ValueError( - f"Kamil ADC frame must be {KAMIL_ADC_FRAME_BYTES} bytes, got {len(frame)}" - ) - marker, step, real, imag = _POINT_FRAME_STRUCT.unpack(frame) - if marker != KAMIL_ADC_MARKER: - raise ValueError(f"Kamil ADC marker mismatch: got 0x{marker:04x}, expected 0x000a") - if step != expected_step: - raise ValueError(f"Kamil ADC step mismatch: got {step}, expected {expected_step}") - return complex(real, imag) +def _parse_point_frame(frame: bytes, expected_step: int) -> complex: + """Parse one 8-byte point frame; validate marker and step ordering.""" + marker, step, real, imag = _POINT_STRUCT.unpack(frame) + if marker != KAMIL_ADC_MARKER: + raise ValueError(f"Kamil ADC marker mismatch: got 0x{marker:04x}, expected 0x000a") + if step != expected_step: + raise ValueError(f"Kamil ADC step mismatch: got {step}, expected {expected_step}") + return complex(real, imag) @dataclass(slots=True) class KamilAdcTtyReader: - """Read full Kamil ADC sweep packets from a nonblocking TTY stream.""" + """Background-thread TTY reader publishing the latest completed sweep. + + The reader spawns a daemon thread on :meth:`open` which continuously + drains the TTY, parses frames into complete sweeps, and stores the most + recent one in a single-slot mailbox. Consumers call :meth:`read_sweep` to + take that sweep; if a newer one arrives before the consumer reads, it + overwrites the previous unread value — by design, since consumers always + want the freshest data. + """ tty_path: str _fd: int | None = field(init=False, default=None, repr=False) - _buffer: bytearray = field(init=False, default_factory=bytearray, repr=False) - _packet_start_pending: bool = field(init=False, default=False, repr=False) + _thread: threading.Thread | None = field(init=False, default=None, repr=False) + _stop_event: threading.Event = field(init=False, default_factory=threading.Event, repr=False) + _mailbox_cv: threading.Condition = field(init=False, default_factory=threading.Condition, repr=False) + _latest_sweep: np.ndarray | None = field(init=False, default=None, repr=False) + _reader_error: Exception | None = field(init=False, default=None, repr=False) + _locked_points: int | None = field(init=False, default=None, repr=False) + _published_count: int = field(init=False, default=0, repr=False) def open(self) -> None: - """Open the configured TTY path for binary reads.""" + """Open the TTY and start the background reader thread.""" if self._fd is not None: return self._fd = os.open(self.tty_path, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK) + self._stop_event.clear() + self._latest_sweep = None + self._reader_error = None + self._locked_points = None + self._published_count = 0 + self._thread = threading.Thread( + target=self._reader_loop, + name=f"kamil-adc-tty-reader[{self.tty_path}]", + daemon=True, + ) + self._thread.start() def close(self) -> None: - """Close the TTY file descriptor.""" - if self._fd is None: - return - try: - os.close(self._fd) - finally: - self._fd = None - self._buffer.clear() - self._packet_start_pending = False + """Stop the reader thread and close the TTY descriptor.""" + self._stop_event.set() + with self._mailbox_cv: + self._mailbox_cv.notify_all() + if self._thread is not None: + self._thread.join(timeout=1.0) + self._thread = None + if self._fd is not None: + try: + os.close(self._fd) + finally: + self._fd = None + self._latest_sweep = None + self._reader_error = None + self._locked_points = None + + @property + def locked_points(self) -> int | None: + """Return the sweep point count established by the first sweep, or `None`.""" + return self._locked_points + + @property + def published_count(self) -> int: + """Return the total number of sweeps the reader thread has produced.""" + with self._mailbox_cv: + return self._published_count def read_sweep( self, *, timeout_s: float, process: subprocess.Popen[bytes] | None = None, - expected_points: int | None = None, ) -> np.ndarray: - """Read one full packet, optionally discarding packets with an unexpected point count.""" - if self._fd is None: + """Wait for and return the next published sweep. + + Raises :class:`TimeoutError` if no sweep arrives within `timeout_s`, + :class:`RuntimeError` if the external collector process exited, and + propagates any exception caught by the reader thread. + """ + if self._thread is None: raise RuntimeError("Kamil ADC TTY reader is not open") - if expected_points is not None: - if expected_points <= 0: - raise ValueError("Kamil ADC expected points must be > 0") - if expected_points > KAMIL_ADC_MAX_STEP: - raise ValueError(f"Kamil ADC expected points must be <= {KAMIL_ADC_MAX_STEP}") - deadline = time.monotonic() + float(timeout_s) - while True: - values = self._read_one_sweep(deadline, process) - if expected_points is None or int(values.size) == int(expected_points): - return values - logger.warning( - "Discarding Kamil ADC sweep with %d points; expected %d", - int(values.size), - int(expected_points), - ) + with self._mailbox_cv: + while True: + # Always deliver a pending sweep first: if the reader thread + # both published a sweep and then died, the consumer should + # still see the good data and only meet the error on the next + # call. + if self._latest_sweep is not None: + sweep = self._latest_sweep + self._latest_sweep = None + return sweep + if self._reader_error is not None: + raise self._reader_error + self._raise_if_process_exited(process) + remaining_s = deadline - time.monotonic() + if remaining_s <= 0.0: + raise TimeoutError( + f"Timed out waiting for Kamil ADC sweep after {float(timeout_s):.3f}s" + ) + # Wake periodically so we can re-check process liveness. + self._mailbox_cv.wait(timeout=min(_READ_POLL_INTERVAL_S, remaining_s)) - def _read_one_sweep( - self, - deadline: float, - process: subprocess.Popen[bytes] | None, - ) -> np.ndarray: - """Read one packet from start marker to the next start marker.""" - if self._packet_start_pending: - self._packet_start_pending = False - else: - self._read_until_packet_start(deadline, process) + # ------------------------------------------------------------------ + # Reader-thread internals + # ------------------------------------------------------------------ + def _reader_loop(self) -> None: + """Drain TTY → parse frames → publish completed sweeps until stop.""" + buffer = bytearray() + try: + if not self._skip_to_first_start_marker(buffer): + return + while not self._stop_event.is_set(): + sweep = self._read_one_sweep(buffer) + if sweep is None: + return + self._publish_sweep(sweep) + except Exception as exc: # noqa: BLE001 — surfaced to the consumer via read_sweep + self._publish_error(exc) + + def _skip_to_first_start_marker(self, buffer: bytearray) -> bool: + """Discard pre-roll bytes until a start marker is consumed from `buffer`.""" + while not self._stop_event.is_set(): + start_index = buffer.find(_START_FRAME) + if start_index >= 0: + del buffer[: start_index + KAMIL_ADC_FRAME_BYTES] + return True + # Keep just enough trailing bytes that a marker split across read + # boundaries can still be reassembled on the next chunk. + if len(buffer) >= KAMIL_ADC_FRAME_BYTES: + del buffer[: -(KAMIL_ADC_FRAME_BYTES - 1)] + if not self._read_more(buffer): + return False + return False + + def _read_one_sweep(self, buffer: bytearray) -> np.ndarray | None: + """Parse frames from `buffer` until the next start marker; return the sweep.""" values: list[complex] = [] expected_step = 1 - while True: - frame = self._read_frame(deadline, process, received_points=len(values)) - if KamilAdcFrameParser.is_packet_start(frame): + while not self._stop_event.is_set(): + while len(buffer) < KAMIL_ADC_FRAME_BYTES: + if not self._read_more(buffer): + return None + frame = bytes(buffer[:KAMIL_ADC_FRAME_BYTES]) + del buffer[:KAMIL_ADC_FRAME_BYTES] + + if frame == _START_FRAME: if not values: + # Two consecutive markers — ignore the empty sweep and keep parsing. continue - self._packet_start_pending = True + self._validate_and_lock_point_count(len(values)) return np.asarray(values, dtype=np.complex64) - if expected_step > KAMIL_ADC_MAX_STEP: - raise RuntimeError(f"Kamil ADC sweep exceeded {KAMIL_ADC_MAX_STEP} points without packet end") - values.append(KamilAdcFrameParser.parse_point(frame, expected_step)) + if self._locked_points is not None and expected_step > self._locked_points: + raise RuntimeError( + f"Kamil ADC sweep exceeded locked point count {self._locked_points} " + "without a start marker" + ) + values.append(_parse_point_frame(frame, expected_step)) expected_step += 1 + return None - def discard_pending(self, process: subprocess.Popen[bytes] | None = None) -> None: - """Discard stale bytes while keeping the newest packet-start boundary.""" - if self._fd is None: - raise RuntimeError("Kamil ADC TTY reader is not open") - self._buffer.clear() - self._packet_start_pending = False - fd = self._require_fd() - while True: - self._raise_if_process_exited(process) + def _validate_and_lock_point_count(self, points: int) -> None: + """Lock the point count on the first sweep; reject mismatches thereafter.""" + if self._locked_points is None: + self._locked_points = points + logger.info("Kamil ADC sweep point count locked to %d", points) + return + if points != self._locked_points: + raise RuntimeError( + f"Kamil ADC sweep length changed: locked={self._locked_points}, got={points}" + ) + + def _read_more(self, buffer: bytearray) -> bool: + """Block on `select` until bytes arrive, then append them to `buffer`. + + Returns `False` if the reader was asked to stop, `True` if at least one + byte was appended. Raises on stream-level errors. + """ + fd = self._fd + if fd is None: + return False + while not self._stop_event.is_set(): try: - readable, _, _ = select.select([fd], [], [], 0.0) + readable, _, _ = select.select([fd], [], [], _READ_POLL_INTERVAL_S) except InterruptedError: continue if not readable: - return + continue try: - chunk = os.read(fd, 4096) + chunk = os.read(fd, _READ_CHUNK_BYTES) except BlockingIOError: - return + continue except OSError as exc: if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}: - return - raise RuntimeError(f"Failed to drain Kamil ADC TTY `{self.tty_path}`: {exc}") from exc + continue + raise RuntimeError( + f"Failed to read Kamil ADC TTY `{self.tty_path}`: {exc}" + ) from exc if not chunk: - raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while draining") - self._buffer.extend(chunk) - self._keep_latest_packet_start_tail() + raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while reading") + buffer.extend(chunk) + return True + return False - def _keep_latest_packet_start_tail(self) -> None: - """Keep only bytes from the latest complete packet-start marker onward.""" - start_index = self._buffer.rfind(_START_FRAME) - if start_index >= 0: - del self._buffer[:start_index] - return - if len(self._buffer) >= KAMIL_ADC_FRAME_BYTES: - del self._buffer[:-KAMIL_ADC_FRAME_BYTES + 1] + def _publish_sweep(self, sweep: np.ndarray) -> None: + """Store `sweep` as the latest mailbox value, overwriting any prior unread one.""" + with self._mailbox_cv: + self._latest_sweep = sweep + self._published_count += 1 + self._mailbox_cv.notify() - def _read_until_packet_start( - self, - deadline: float, - process: subprocess.Popen[bytes] | None, - ) -> None: - while True: - start_index = self._buffer.find(_START_FRAME) - if start_index >= 0: - del self._buffer[: start_index + KAMIL_ADC_FRAME_BYTES] - return - if len(self._buffer) >= KAMIL_ADC_FRAME_BYTES: - del self._buffer[:-KAMIL_ADC_FRAME_BYTES + 1] - self._read_available(deadline, process) - - def _read_frame( - self, - deadline: float, - process: subprocess.Popen[bytes] | None, - *, - received_points: int, - expected_points: int | None = None, - ) -> bytes: - while len(self._buffer) < KAMIL_ADC_FRAME_BYTES: - self._read_available(deadline, process, received_points, expected_points) - frame = bytes(self._buffer[:KAMIL_ADC_FRAME_BYTES]) - del self._buffer[:KAMIL_ADC_FRAME_BYTES] - return frame - - def _read_available( - self, - deadline: float, - process: subprocess.Popen[bytes] | None, - received_points: int | None = None, - expected_points: int | None = None, - ) -> None: - self._raise_if_process_exited(process) - remaining_s = deadline - time.monotonic() - if remaining_s <= 0.0: - if received_points is None or expected_points is None: - if received_points is not None: - raise TimeoutError( - f"Timed out waiting for Kamil ADC sweep end: received {received_points} points" - ) - raise TimeoutError("Timed out waiting for Kamil ADC packet-start marker") - raise TimeoutError( - f"Timed out waiting for Kamil ADC sweep: received {received_points}/{expected_points} points" - ) - - fd = self._require_fd() - wait_s = min(0.05, remaining_s) - try: - readable, _, _ = select.select([fd], [], [], wait_s) - except InterruptedError: - return - if not readable: - return - - try: - chunk = os.read(fd, 4096) - except BlockingIOError: - return - except OSError as exc: - if exc.errno in {errno.EAGAIN, errno.EWOULDBLOCK}: - return - raise RuntimeError(f"Failed to read Kamil ADC TTY `{self.tty_path}`: {exc}") from exc - if not chunk: - raise RuntimeError(f"Kamil ADC TTY `{self.tty_path}` closed while reading") - self._buffer.extend(chunk) - - def _require_fd(self) -> int: - if self._fd is None: - raise RuntimeError("Kamil ADC TTY reader is not open") - return self._fd + def _publish_error(self, exc: Exception) -> None: + """Record `exc` as the reader fault and wake any waiter.""" + with self._mailbox_cv: + self._reader_error = exc + self._mailbox_cv.notify_all() @staticmethod def _raise_if_process_exited(process: subprocess.Popen[bytes] | None) -> None: @@ -261,14 +302,13 @@ class KamilAdcTtyReader: @dataclass(slots=True) class KamilAdcService: - """Launch `kamil_adc` and acquire TTY sweeps.""" + """Launch the external `kamil_adc` collector and serve its sweeps.""" config: RunConfigModel _process: subprocess.Popen[bytes] | None = field(init=False, default=None, repr=False) _reader: KamilAdcTtyReader | None = field(init=False, default=None, repr=False) _settings: RadarSweepModel | None = field(init=False, default=None, repr=False) _frequency_hz: np.ndarray | None = field(init=False, default=None, repr=False) - _expected_points: int | None = field(init=False, default=None, repr=False) def __post_init__(self) -> None: self._validate_config() @@ -281,10 +321,9 @@ class KamilAdcService: return [executable_path, *adc.args, f"tty:{adc.tty_path}"] def open(self) -> None: - """Launch the collector and open its TTY stream.""" + """Launch the collector and start the TTY reader thread.""" if self._reader is not None: return - previous_tty_identity = _prepare_tty_path_for_collector(self.config.radar.kamil_adc.tty_path) try: self._start_process() @@ -297,50 +336,40 @@ class KamilAdcService: raise def close(self) -> None: - """Close TTY and stop the external collector.""" + """Stop the TTY reader and the external collector process.""" if self._reader is not None: with suppress(Exception): self._reader.close() self._reader = None - self._stop_process() def configure(self, sweep: RadarSweepModel) -> None: - """Store sweep settings used to construct the synthetic frequency axis.""" + """Store sweep settings used to build the synthetic frequency axis.""" self._validate_sweep(sweep) self._settings = sweep self._frequency_hz = None - self._expected_points = None def read_device_limits(self) -> dict[str, float | int]: """Kamil ADC has no runtime-readable sweep limit API.""" raise RuntimeError("Kamil ADC device limits are not available") def acquire(self) -> SweepResult: - """Acquire one Kamil ADC sweep as S21; fill S11 with explicit zeros.""" + """Return the most recent completed sweep as S21 (S11 filled with zeros).""" if self._settings is None: raise RuntimeError("Kamil ADC service is not configured") if self._reader is None: raise RuntimeError("Kamil ADC service is not open") process = self._process if process is None or process.poll() is not None: - code = None if process is None else process.poll() - raise RuntimeError(f"Kamil ADC process is not running (code={code})") + return_code = None if process is None else process.poll() + raise RuntimeError(f"Kamil ADC process is not running (code={return_code})") - self._reader.discard_pending(process) s21 = self._reader.read_sweep( timeout_s=self.config.radar.kamil_adc.sweep_timeout_s, process=process, - expected_points=self._expected_points, ) points = int(s21.size) - if points <= 0: - raise RuntimeError("Kamil ADC sweep contained no points") - if self._expected_points is None: - self._expected_points = points - self._frequency_hz = self._build_frequency_axis(points) - logger.info("Kamil ADC sweep point count locked to %d", points) - if self._frequency_hz is None: + if self._frequency_hz is None or self._frequency_hz.size != points: self._frequency_hz = self._build_frequency_axis(points) return SweepResult( x=self._frequency_hz.copy(), @@ -350,10 +379,13 @@ class KamilAdcService: }, ) + # ------------------------------------------------------------------ + # Process / TTY lifecycle + # ------------------------------------------------------------------ + def _start_process(self) -> None: if self._process is not None and self._process.poll() is None: return - adc = self.config.radar.kamil_adc env = os.environ.copy() env.update(adc.env) @@ -371,11 +403,8 @@ class KamilAdcService: def _stop_process(self) -> None: process = self._process self._process = None - if process is None: + if process is None or process.poll() is not None: return - if process.poll() is not None: - return - with suppress(ProcessLookupError): os.killpg(process.pid, signal.SIGTERM) try: @@ -383,7 +412,6 @@ class KamilAdcService: return except subprocess.TimeoutExpired: pass - with suppress(ProcessLookupError): os.killpg(process.pid, signal.SIGKILL) process.wait(timeout=1.0) @@ -401,6 +429,10 @@ class KamilAdcService: f"Timed out waiting for Kamil ADC TTY `{adc.tty_path}` to be created by the collector" ) + # ------------------------------------------------------------------ + # Validation helpers + # ------------------------------------------------------------------ + def _validate_config(self) -> None: if not self.config.is_kamil_adc: raise RuntimeError("KamilAdcService requires radar.model='kamil_adc'") diff --git a/python_app/hardware_full/librevna_multi_device_driver/controller.py b/python_app/hardware_full/librevna_multi_device_driver/controller.py index 22c1e23..898bd30 100644 --- a/python_app/hardware_full/librevna_multi_device_driver/controller.py +++ b/python_app/hardware_full/librevna_multi_device_driver/controller.py @@ -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) diff --git a/python_app/hardware_full/librevna_multi_device_driver/cycle_collection.py b/python_app/hardware_full/librevna_multi_device_driver/cycle_collection.py index a0f2b1b..cb30d2b 100644 --- a/python_app/hardware_full/librevna_multi_device_driver/cycle_collection.py +++ b/python_app/hardware_full/librevna_multi_device_driver/cycle_collection.py @@ -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] diff --git a/python_app/hardware_full/multi_device_service.py b/python_app/hardware_full/multi_device_service.py index 100d14c..c9e71d4 100644 --- a/python_app/hardware_full/multi_device_service.py +++ b/python_app/hardware_full/multi_device_service.py @@ -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() - self._controller = None + """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) - self.open() + + 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, ) - self.recover() + # 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 diff --git a/python_app/hardware_full/sn9000_service.py b/python_app/hardware_full/sn9000_service.py index 663e0ac..121b8ab 100644 --- a/python_app/hardware_full/sn9000_service.py +++ b/python_app/hardware_full/sn9000_service.py @@ -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: diff --git a/python_app/models/gui_profile_codec.py b/python_app/models/gui_profile_codec.py index 5d3a142..3e83d47 100644 --- a/python_app/models/gui_profile_codec.py +++ b/python_app/models/gui_profile_codec.py @@ -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, diff --git a/python_app/models/gui_profile_schema.py b/python_app/models/gui_profile_schema.py index 646459f..01deb93 100644 --- a/python_app/models/gui_profile_schema.py +++ b/python_app/models/gui_profile_schema.py @@ -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 diff --git a/python_app/models/run_config_codec.py b/python_app/models/run_config_codec.py index 07b54fe..a3a2b3d 100644 --- a/python_app/models/run_config_codec.py +++ b/python_app/models/run_config_codec.py @@ -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]: diff --git a/python_app/orchestration/gui_session_state.py b/python_app/orchestration/gui_session_state.py index 12377a2..7ac7b12 100644 --- a/python_app/orchestration/gui_session_state.py +++ b/python_app/orchestration/gui_session_state.py @@ -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() - payload = json.loads(self._path.read_text(encoding="utf-8")) + 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: diff --git a/python_app/orchestration/live_processing_config.py b/python_app/orchestration/live_processing_config.py index c8800ae..78ce2f6 100644 --- a/python_app/orchestration/live_processing_config.py +++ b/python_app/orchestration/live_processing_config.py @@ -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), diff --git a/python_app/scripts/matrix_raw_producer.py b/python_app/scripts/matrix_raw_producer.py index 795a736..6d0cd7d 100644 --- a/python_app/scripts/matrix_raw_producer.py +++ b/python_app/scripts/matrix_raw_producer.py @@ -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() - collection = radar.acquire_collection(collection_id=collection_id) + 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,7 +141,9 @@ def main() -> int: ) collection_id += 1 finally: - radar.close() + if radar is not None: + with suppress(Exception): + radar.close() raw_tap_writer.close() raw_writer.close() diff --git a/python_app/storage/npz/store.py b/python_app/storage/npz/store.py index 7e5789c..b6d1354 100644 --- a/python_app/storage/npz/store.py +++ b/python_app/storage/npz/store.py @@ -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,30 +99,34 @@ 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}") - meta = json.loads(meta_path.read_text(encoding="utf-8")) - arrays = np.load(npz_path) + set_label = f"{kind}/{radar_key}/{set_name}" + try: + meta = json.loads(meta_path.read_text(encoding="utf-8")) + arrays = np.load(npz_path, allow_pickle=False) - traces: list[TraceData] = [] - for combo in meta["combos"]: - freq = np.asarray(arrays[combo["freq_key"]], dtype=np.float32) - s11 = np.asarray(arrays[combo["s11_key"]], dtype=np.complex64) - s21 = np.asarray(arrays[combo["s21_key"]], dtype=np.complex64) - traces.append( - TraceData( - combo=ComboKey(input=int(combo["input"]), output=int(combo["output"])), - frequency_hz=freq, - s11=s11, - s21=s21, + traces: list[TraceData] = [] + for combo in meta["combos"]: + freq = np.asarray(arrays[combo["freq_key"]], dtype=np.float32) + s11 = np.asarray(arrays[combo["s11_key"]], dtype=np.complex64) + s21 = np.asarray(arrays[combo["s21_key"]], dtype=np.complex64) + traces.append( + TraceData( + combo=ComboKey(input=int(combo["input"]), output=int(combo["output"])), + frequency_hz=freq, + s11=s11, + s21=s21, + ) ) - ) - return SweepCollection( - collection_id=int(meta["collection_id"]), - monotonic_ns=int(meta["monotonic_ns"]), - traces=traces, - capture_start_ns=int(meta.get("capture_start_ns", 0)), - capture_end_ns=int(meta.get("capture_end_ns", 0)), - ) + return SweepCollection( + collection_id=int(meta["collection_id"]), + monotonic_ns=int(meta["monotonic_ns"]), + traces=traces, + 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)`.""" diff --git a/python_app/tests/test_kamil_adc_service.py b/python_app/tests/test_kamil_adc_service.py index 0c1ea3f..5379892 100644 --- a/python_app/tests/test_kamil_adc_service.py +++ b/python_app/tests/test_kamil_adc_service.py @@ -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(" 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 + 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: - tty.setraw(slave_fd) - reader = KamilAdcTtyReader(os.ttyname(slave_fd)) - reader.open() 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): diff --git a/python_app/workflows/multi_radar_capture_workflow.py b/python_app/workflows/multi_radar_capture_workflow.py index e462d7b..3707fd6 100644 --- a/python_app/workflows/multi_radar_capture_workflow.py +++ b/python_app/workflows/multi_radar_capture_workflow.py @@ -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: diff --git a/python_app/workflows/sequential_capture_workflow.py b/python_app/workflows/sequential_capture_workflow.py index f7413f3..9338cbc 100644 --- a/python_app/workflows/sequential_capture_workflow.py +++ b/python_app/workflows/sequential_capture_workflow.py @@ -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: