feat: Raw IQ Replay mode — software FPGA signal chain with playback controls
Add a 4th connection mode to the V7 dashboard that loads raw complex IQ captures (.npy) and runs the full FPGA signal processing chain in software: quantize → AGC → Range FFT → Doppler FFT → MTI → DC notch → CFAR. Implementation (7 steps): - v7/agc_sim.py: bit-accurate AGC runtime extracted from adi_agc_analysis.py - v7/processing.py: RawIQFrameProcessor (full signal chain) + shared extract_targets_from_frame() for bin-to-physical conversion - v7/raw_iq_replay.py: RawIQReplayController with thread-safe playback state machine (play/pause/stop/step/seek/loop/FPS) - v7/workers.py: RawIQReplayWorker (QThread) emitting same signals as RadarDataWorker + playback state/index signals - v7/dashboard.py: mode combo entry, playback controls UI, dynamic RangeDopplerCanvas that adapts to any frame size Bug fixes included: - RangeDopplerCanvas no longer hardcodes 64x32; resizes dynamically - Doppler centre bin uses n_doppler//2 instead of hardcoded 16 - Shared target extraction eliminates duplicate code between workers Ruff clean, 120/120 tests pass.
This commit is contained in:
Jason
@@ -0,0 +1,222 @@
|
||||
"""
|
||||
v7.agc_sim -- Bit-accurate AGC simulation matching rx_gain_control.v.
|
||||
|
||||
Provides stateful, frame-by-frame AGC processing for the Raw IQ Replay
|
||||
mode and offline analysis. All gain encoding, clamping, and attack/decay/
|
||||
holdoff logic is identical to the FPGA RTL.
|
||||
|
||||
Classes:
|
||||
- AGCState -- mutable internal AGC state (gain, holdoff counter)
|
||||
- AGCFrameResult -- per-frame AGC metrics after processing
|
||||
|
||||
Functions:
|
||||
- signed_to_encoding -- signed gain (-7..+7) -> 4-bit encoding
|
||||
- encoding_to_signed -- 4-bit encoding -> signed gain
|
||||
- clamp_gain -- clamp to [-7, +7]
|
||||
- apply_gain_shift -- apply gain_shift to 16-bit IQ arrays
|
||||
- process_agc_frame -- run one frame through AGC, update state
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# FPGA AGC parameters (rx_gain_control.v reset defaults)
|
||||
# ---------------------------------------------------------------------------
|
||||
AGC_TARGET_DEFAULT = 200 # host_agc_target (8-bit)
|
||||
AGC_ATTACK_DEFAULT = 1 # host_agc_attack (4-bit)
|
||||
AGC_DECAY_DEFAULT = 1 # host_agc_decay (4-bit)
|
||||
AGC_HOLDOFF_DEFAULT = 4 # host_agc_holdoff (4-bit)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Gain encoding helpers (match RTL signed_to_encoding / encoding_to_signed)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def signed_to_encoding(g: int) -> int:
|
||||
"""Convert signed gain (-7..+7) to gain_shift[3:0] encoding.
|
||||
|
||||
[3]=0, [2:0]=N -> amplify (left shift) by N
|
||||
[3]=1, [2:0]=N -> attenuate (right shift) by N
|
||||
"""
|
||||
if g >= 0:
|
||||
return g & 0x07
|
||||
return 0x08 | ((-g) & 0x07)
|
||||
|
||||
|
||||
def encoding_to_signed(enc: int) -> int:
|
||||
"""Convert gain_shift[3:0] encoding to signed gain."""
|
||||
if (enc & 0x08) == 0:
|
||||
return enc & 0x07
|
||||
return -(enc & 0x07)
|
||||
|
||||
|
||||
def clamp_gain(val: int) -> int:
|
||||
"""Clamp to [-7, +7] (matches RTL clamp_gain function)."""
|
||||
return max(-7, min(7, val))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Apply gain shift to IQ data (matches RTL combinational logic)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def apply_gain_shift(
|
||||
frame_i: np.ndarray,
|
||||
frame_q: np.ndarray,
|
||||
gain_enc: int,
|
||||
) -> tuple[np.ndarray, np.ndarray, int]:
|
||||
"""Apply gain_shift encoding to 16-bit signed IQ arrays.
|
||||
|
||||
Returns (shifted_i, shifted_q, overflow_count).
|
||||
Matches the RTL: left shift = amplify, right shift = attenuate,
|
||||
saturate to +/-32767 on overflow.
|
||||
"""
|
||||
direction = (gain_enc >> 3) & 1 # 0=amplify, 1=attenuate
|
||||
amount = gain_enc & 0x07
|
||||
|
||||
if amount == 0:
|
||||
return frame_i.copy(), frame_q.copy(), 0
|
||||
|
||||
if direction == 0:
|
||||
# Left shift (amplify)
|
||||
si = frame_i.astype(np.int64) * (1 << amount)
|
||||
sq = frame_q.astype(np.int64) * (1 << amount)
|
||||
else:
|
||||
# Arithmetic right shift (attenuate)
|
||||
si = frame_i.astype(np.int64) >> amount
|
||||
sq = frame_q.astype(np.int64) >> amount
|
||||
|
||||
# Count overflows (post-shift values outside 16-bit signed range)
|
||||
overflow_i = (si > 32767) | (si < -32768)
|
||||
overflow_q = (sq > 32767) | (sq < -32768)
|
||||
overflow_count = int((overflow_i | overflow_q).sum())
|
||||
|
||||
# Saturate to +/-32767
|
||||
si = np.clip(si, -32768, 32767).astype(np.int16)
|
||||
sq = np.clip(sq, -32768, 32767).astype(np.int16)
|
||||
|
||||
return si, sq, overflow_count
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# AGC state and per-frame result dataclasses
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@dataclass
|
||||
class AGCConfig:
|
||||
"""AGC tuning parameters (mirrors FPGA host registers 0x28-0x2C)."""
|
||||
|
||||
enabled: bool = False
|
||||
target: int = AGC_TARGET_DEFAULT # 8-bit peak target
|
||||
attack: int = AGC_ATTACK_DEFAULT # 4-bit attenuation step
|
||||
decay: int = AGC_DECAY_DEFAULT # 4-bit gain-up step
|
||||
holdoff: int = AGC_HOLDOFF_DEFAULT # 4-bit frames to hold
|
||||
|
||||
|
||||
@dataclass
|
||||
class AGCState:
|
||||
"""Mutable internal AGC state — persists across frames."""
|
||||
|
||||
gain: int = 0 # signed gain, -7..+7
|
||||
holdoff_counter: int = 0 # frames remaining before gain-up allowed
|
||||
was_enabled: bool = False # tracks enable transitions
|
||||
|
||||
|
||||
@dataclass
|
||||
class AGCFrameResult:
|
||||
"""Per-frame AGC metrics returned by process_agc_frame()."""
|
||||
|
||||
gain_enc: int = 0 # gain_shift[3:0] encoding applied this frame
|
||||
gain_signed: int = 0 # signed gain for display
|
||||
peak_mag_8bit: int = 0 # pre-gain peak magnitude (upper 8 of 15 bits)
|
||||
saturation_count: int = 0 # post-gain overflow count (clamped to 255)
|
||||
overflow_raw: int = 0 # raw overflow count (unclamped)
|
||||
shifted_i: np.ndarray = field(default_factory=lambda: np.array([], dtype=np.int16))
|
||||
shifted_q: np.ndarray = field(default_factory=lambda: np.array([], dtype=np.int16))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Per-frame AGC processing (bit-accurate to rx_gain_control.v)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def quantize_iq(frame: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Quantize complex IQ to 16-bit signed I and Q arrays.
|
||||
|
||||
Input: 2-D complex array (chirps x samples) — any complex dtype.
|
||||
Output: (frame_i, frame_q) as int16.
|
||||
"""
|
||||
frame_i = np.clip(np.round(frame.real), -32768, 32767).astype(np.int16)
|
||||
frame_q = np.clip(np.round(frame.imag), -32768, 32767).astype(np.int16)
|
||||
return frame_i, frame_q
|
||||
|
||||
|
||||
def process_agc_frame(
|
||||
frame_i: np.ndarray,
|
||||
frame_q: np.ndarray,
|
||||
config: AGCConfig,
|
||||
state: AGCState,
|
||||
) -> AGCFrameResult:
|
||||
"""Run one frame through the FPGA AGC inner loop.
|
||||
|
||||
Mutates *state* in place (gain and holdoff_counter).
|
||||
Returns AGCFrameResult with metrics and shifted IQ data.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
frame_i, frame_q : int16 arrays (any shape, typically chirps x samples)
|
||||
config : AGC tuning parameters
|
||||
state : mutable AGC state from previous frame
|
||||
"""
|
||||
# --- PRE-gain peak measurement (RTL lines 133-135, 211-213) ---
|
||||
abs_i = np.abs(frame_i.astype(np.int32))
|
||||
abs_q = np.abs(frame_q.astype(np.int32))
|
||||
max_iq = np.maximum(abs_i, abs_q)
|
||||
frame_peak_15bit = int(max_iq.max()) if max_iq.size > 0 else 0
|
||||
peak_8bit = (frame_peak_15bit >> 7) & 0xFF
|
||||
|
||||
# --- Handle AGC enable transition (RTL lines 250-253) ---
|
||||
if config.enabled and not state.was_enabled:
|
||||
state.gain = 0
|
||||
state.holdoff_counter = config.holdoff
|
||||
state.was_enabled = config.enabled
|
||||
|
||||
# --- Determine effective gain encoding ---
|
||||
if config.enabled:
|
||||
effective_enc = signed_to_encoding(state.gain)
|
||||
else:
|
||||
effective_enc = signed_to_encoding(state.gain)
|
||||
|
||||
# --- Apply gain shift + count POST-gain overflow ---
|
||||
shifted_i, shifted_q, overflow_raw = apply_gain_shift(
|
||||
frame_i, frame_q, effective_enc)
|
||||
sat_count = min(255, overflow_raw)
|
||||
|
||||
# --- AGC update at frame boundary (RTL lines 226-246) ---
|
||||
if config.enabled:
|
||||
if sat_count > 0:
|
||||
# Clipping: reduce gain immediately (attack)
|
||||
state.gain = clamp_gain(state.gain - config.attack)
|
||||
state.holdoff_counter = config.holdoff
|
||||
elif peak_8bit < config.target:
|
||||
# Signal too weak: increase gain after holdoff
|
||||
if state.holdoff_counter == 0:
|
||||
state.gain = clamp_gain(state.gain + config.decay)
|
||||
else:
|
||||
state.holdoff_counter -= 1
|
||||
else:
|
||||
# Good range (peak >= target, no sat): hold, reset holdoff
|
||||
state.holdoff_counter = config.holdoff
|
||||
|
||||
return AGCFrameResult(
|
||||
gain_enc=effective_enc,
|
||||
gain_signed=state.gain if config.enabled else encoding_to_signed(effective_enc),
|
||||
peak_mag_8bit=peak_8bit,
|
||||
saturation_count=sat_count,
|
||||
overflow_raw=overflow_raw,
|
||||
shifted_i=shifted_i,
|
||||
shifted_q=shifted_q,
|
||||
)
|
||||
@@ -25,6 +25,7 @@ commands sent over FT2232H.
|
||||
import time
|
||||
import logging
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -59,8 +60,10 @@ from .hardware import (
|
||||
DataRecorder,
|
||||
STM32USBInterface,
|
||||
)
|
||||
from .processing import RadarProcessor, USBPacketParser
|
||||
from .workers import RadarDataWorker, GPSDataWorker, TargetSimulator
|
||||
from .processing import RadarProcessor, USBPacketParser, RawIQFrameProcessor
|
||||
from .workers import RadarDataWorker, RawIQReplayWorker, GPSDataWorker, TargetSimulator
|
||||
from .raw_iq_replay import RawIQReplayController, PlaybackState
|
||||
from .agc_sim import AGCConfig
|
||||
from .map_widget import RadarMapWidget
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
@@ -75,19 +78,29 @@ NUM_DOPPLER_BINS = 32
|
||||
# =============================================================================
|
||||
|
||||
class RangeDopplerCanvas(FigureCanvasQTAgg):
|
||||
"""Matplotlib canvas showing the 64x32 Range-Doppler map with dark theme."""
|
||||
"""Matplotlib canvas showing a Range-Doppler map with dark theme.
|
||||
|
||||
Adapts dynamically to incoming frame dimensions (e.g. 64x32 from FPGA,
|
||||
or different sizes from Raw IQ Replay).
|
||||
"""
|
||||
|
||||
def __init__(self, _parent=None):
|
||||
fig = Figure(figsize=(10, 6), facecolor=DARK_BG)
|
||||
self.ax = fig.add_subplot(111, facecolor=DARK_ACCENT)
|
||||
|
||||
self._data = np.zeros((NUM_RANGE_BINS, NUM_DOPPLER_BINS))
|
||||
# Initial backing data — will resize on first update_map call
|
||||
self._n_range = NUM_RANGE_BINS
|
||||
self._n_doppler = NUM_DOPPLER_BINS
|
||||
self._data = np.zeros((self._n_range, self._n_doppler))
|
||||
self.im = self.ax.imshow(
|
||||
self._data, aspect="auto", cmap="hot",
|
||||
extent=[0, NUM_DOPPLER_BINS, 0, NUM_RANGE_BINS], origin="lower",
|
||||
extent=[0, self._n_doppler, 0, self._n_range], origin="lower",
|
||||
)
|
||||
|
||||
self.ax.set_title("Range-Doppler Map (64x32)", color=DARK_FG)
|
||||
self.ax.set_title(
|
||||
f"Range-Doppler Map ({self._n_range}x{self._n_doppler})",
|
||||
color=DARK_FG,
|
||||
)
|
||||
self.ax.set_xlabel("Doppler Bin", color=DARK_FG)
|
||||
self.ax.set_ylabel("Range Bin", color=DARK_FG)
|
||||
self.ax.tick_params(colors=DARK_FG)
|
||||
@@ -98,7 +111,20 @@ class RangeDopplerCanvas(FigureCanvasQTAgg):
|
||||
super().__init__(fig)
|
||||
|
||||
def update_map(self, magnitude: np.ndarray, _detections: np.ndarray = None):
|
||||
"""Update the heatmap with new magnitude data."""
|
||||
"""Update the heatmap with new magnitude data.
|
||||
|
||||
Automatically resizes the canvas if the incoming shape differs from
|
||||
the current backing array.
|
||||
"""
|
||||
nr, nd = magnitude.shape
|
||||
if nr != self._n_range or nd != self._n_doppler:
|
||||
self._n_range = nr
|
||||
self._n_doppler = nd
|
||||
self._data = np.zeros((nr, nd))
|
||||
self.im.set_extent([0, nd, 0, nr])
|
||||
self.ax.set_title(
|
||||
f"Range-Doppler Map ({nr}x{nd})", color=DARK_FG)
|
||||
|
||||
display = np.log10(magnitude + 1)
|
||||
self.im.set_data(display)
|
||||
self.im.set_clim(vmin=display.min(), vmax=max(display.max(), 0.1))
|
||||
@@ -142,6 +168,11 @@ class RadarDashboard(QMainWindow):
|
||||
self._gps_worker: GPSDataWorker | None = None
|
||||
self._simulator: TargetSimulator | None = None
|
||||
|
||||
# Raw IQ Replay
|
||||
self._replay_controller: RawIQReplayController | None = None
|
||||
self._replay_worker: RawIQReplayWorker | None = None
|
||||
self._iq_processor: RawIQFrameProcessor | None = None
|
||||
|
||||
# State
|
||||
self._running = False
|
||||
self._demo_mode = False
|
||||
@@ -341,7 +372,8 @@ class RadarDashboard(QMainWindow):
|
||||
# Row 0: connection mode + device combos + buttons
|
||||
ctrl_layout.addWidget(QLabel("Mode:"), 0, 0)
|
||||
self._mode_combo = QComboBox()
|
||||
self._mode_combo.addItems(["Mock", "Live FT2232H", "Replay (.npy)"])
|
||||
self._mode_combo.addItems([
|
||||
"Mock", "Live FT2232H", "Replay (.npy)", "Raw IQ Replay (.npy)"])
|
||||
self._mode_combo.setCurrentIndex(0)
|
||||
ctrl_layout.addWidget(self._mode_combo, 0, 1)
|
||||
|
||||
@@ -390,6 +422,55 @@ class RadarDashboard(QMainWindow):
|
||||
self._status_label_main.setAlignment(Qt.AlignmentFlag.AlignRight)
|
||||
ctrl_layout.addWidget(self._status_label_main, 1, 5, 1, 5)
|
||||
|
||||
# Row 2: Raw IQ playback controls (hidden until Raw IQ mode active)
|
||||
self._playback_frame = QFrame()
|
||||
self._playback_frame.setStyleSheet(
|
||||
f"background-color: {DARK_HIGHLIGHT}; border-radius: 4px;")
|
||||
pb_layout = QHBoxLayout(self._playback_frame)
|
||||
pb_layout.setContentsMargins(8, 4, 8, 4)
|
||||
|
||||
self._pb_play_btn = QPushButton("Play")
|
||||
self._pb_play_btn.setStyleSheet(
|
||||
f"QPushButton {{ background-color: {DARK_SUCCESS}; color: white; }}")
|
||||
self._pb_play_btn.clicked.connect(self._pb_play_pause)
|
||||
pb_layout.addWidget(self._pb_play_btn)
|
||||
|
||||
self._pb_step_btn = QPushButton("Step")
|
||||
self._pb_step_btn.clicked.connect(self._pb_step)
|
||||
pb_layout.addWidget(self._pb_step_btn)
|
||||
|
||||
self._pb_stop_btn = QPushButton("Stop")
|
||||
self._pb_stop_btn.setStyleSheet(
|
||||
f"QPushButton {{ background-color: {DARK_ERROR}; color: white; }}")
|
||||
self._pb_stop_btn.clicked.connect(self._stop_radar)
|
||||
pb_layout.addWidget(self._pb_stop_btn)
|
||||
|
||||
pb_layout.addWidget(QLabel("FPS:"))
|
||||
self._pb_fps_spin = QDoubleSpinBox()
|
||||
self._pb_fps_spin.setRange(0.1, 60.0)
|
||||
self._pb_fps_spin.setValue(10.0)
|
||||
self._pb_fps_spin.setSingleStep(1.0)
|
||||
self._pb_fps_spin.valueChanged.connect(self._pb_fps_changed)
|
||||
pb_layout.addWidget(self._pb_fps_spin)
|
||||
|
||||
self._pb_loop_check = QCheckBox("Loop")
|
||||
self._pb_loop_check.setChecked(True)
|
||||
self._pb_loop_check.toggled.connect(self._pb_loop_changed)
|
||||
pb_layout.addWidget(self._pb_loop_check)
|
||||
|
||||
self._pb_frame_label = QLabel("Frame: 0 / 0")
|
||||
self._pb_frame_label.setStyleSheet(
|
||||
f"color: {DARK_INFO}; font-weight: bold;")
|
||||
pb_layout.addWidget(self._pb_frame_label)
|
||||
|
||||
self._pb_file_label = QLabel("")
|
||||
self._pb_file_label.setStyleSheet(f"color: {DARK_TEXT}; font-size: 10px;")
|
||||
pb_layout.addWidget(self._pb_file_label)
|
||||
|
||||
pb_layout.addStretch()
|
||||
self._playback_frame.setVisible(False)
|
||||
ctrl_layout.addWidget(self._playback_frame, 2, 0, 1, 10)
|
||||
|
||||
layout.addWidget(ctrl)
|
||||
|
||||
# ---- Display area (range-doppler + targets table) ------------------
|
||||
@@ -1194,6 +1275,10 @@ class RadarDashboard(QMainWindow):
|
||||
try:
|
||||
mode = self._mode_combo.currentText()
|
||||
|
||||
if "Raw IQ" in mode:
|
||||
self._start_raw_iq_replay()
|
||||
return
|
||||
|
||||
if "Mock" in mode:
|
||||
self._connection = FT2232HConnection(mock=True)
|
||||
if not self._connection.open():
|
||||
@@ -1271,6 +1356,16 @@ class RadarDashboard(QMainWindow):
|
||||
self._radar_worker.wait(2000)
|
||||
self._radar_worker = None
|
||||
|
||||
# Raw IQ Replay cleanup
|
||||
if self._replay_controller is not None:
|
||||
self._replay_controller.stop()
|
||||
if self._replay_worker is not None:
|
||||
self._replay_worker.stop()
|
||||
self._replay_worker.wait(2000)
|
||||
self._replay_worker = None
|
||||
self._replay_controller = None
|
||||
self._iq_processor = None
|
||||
|
||||
if self._gps_worker:
|
||||
self._gps_worker.stop()
|
||||
self._gps_worker.wait(2000)
|
||||
@@ -1285,11 +1380,162 @@ class RadarDashboard(QMainWindow):
|
||||
self._start_btn.setEnabled(True)
|
||||
self._stop_btn.setEnabled(False)
|
||||
self._mode_combo.setEnabled(True)
|
||||
self._playback_frame.setVisible(False)
|
||||
self._status_label_main.setText("Status: Radar stopped")
|
||||
self._sb_status.setText("Radar stopped")
|
||||
self._sb_mode.setText("Idle")
|
||||
logger.info("Radar system stopped")
|
||||
|
||||
# =====================================================================
|
||||
# Raw IQ Replay
|
||||
# =====================================================================
|
||||
|
||||
def _start_raw_iq_replay(self):
|
||||
"""Start raw IQ replay mode: load .npy file and begin playback."""
|
||||
from PyQt6.QtWidgets import QFileDialog
|
||||
|
||||
npy_path, _ = QFileDialog.getOpenFileName(
|
||||
self, "Select Raw IQ .npy file", "",
|
||||
"NumPy files (*.npy);;All files (*)")
|
||||
if not npy_path:
|
||||
return
|
||||
|
||||
try:
|
||||
# Create controller and load file
|
||||
self._replay_controller = RawIQReplayController()
|
||||
info = self._replay_controller.load_file(npy_path)
|
||||
|
||||
# Create frame processor
|
||||
self._iq_processor = RawIQFrameProcessor(
|
||||
n_range_out=min(64, info.n_samples),
|
||||
n_doppler_out=min(32, info.n_chirps),
|
||||
)
|
||||
|
||||
# Apply current AGC settings from FPGA Control tab
|
||||
agc_enable = self._param_spins.get("0x28")
|
||||
agc_target = self._param_spins.get("0x29")
|
||||
agc_attack = self._param_spins.get("0x2A")
|
||||
agc_decay = self._param_spins.get("0x2B")
|
||||
agc_holdoff = self._param_spins.get("0x2C")
|
||||
self._iq_processor.set_agc_config(AGCConfig(
|
||||
enabled=bool(agc_enable.value()) if agc_enable else False,
|
||||
target=agc_target.value() if agc_target else 200,
|
||||
attack=agc_attack.value() if agc_attack else 1,
|
||||
decay=agc_decay.value() if agc_decay else 1,
|
||||
holdoff=agc_holdoff.value() if agc_holdoff else 4,
|
||||
))
|
||||
|
||||
# Apply CFAR settings
|
||||
cfar_en = self._param_spins.get("0x25")
|
||||
cfar_guard = self._param_spins.get("0x21")
|
||||
cfar_train = self._param_spins.get("0x22")
|
||||
cfar_alpha = self._param_spins.get("0x23")
|
||||
cfar_mode = self._param_spins.get("0x24")
|
||||
self._iq_processor.set_cfar_params(
|
||||
enabled=bool(cfar_en.value()) if cfar_en else False,
|
||||
guard=cfar_guard.value() if cfar_guard else 2,
|
||||
train=cfar_train.value() if cfar_train else 8,
|
||||
alpha_q44=cfar_alpha.value() if cfar_alpha else 0x30,
|
||||
mode=cfar_mode.value() if cfar_mode else 0,
|
||||
)
|
||||
|
||||
# Apply MTI / DC notch
|
||||
mti_en = self._param_spins.get("0x26")
|
||||
dc_notch = self._param_spins.get("0x27")
|
||||
self._iq_processor.set_mti_enabled(
|
||||
bool(mti_en.value()) if mti_en else False)
|
||||
self._iq_processor.set_dc_notch_width(
|
||||
dc_notch.value() if dc_notch else 0)
|
||||
|
||||
# Threshold
|
||||
thresh = self._param_spins.get("0x03")
|
||||
self._iq_processor.set_detect_threshold(
|
||||
thresh.value() if thresh else 10000)
|
||||
|
||||
# Create worker
|
||||
self._replay_worker = RawIQReplayWorker(
|
||||
controller=self._replay_controller,
|
||||
processor=self._iq_processor,
|
||||
host_processor=self._processor,
|
||||
settings=self._settings,
|
||||
)
|
||||
self._replay_worker.frameReady.connect(self._on_frame_ready)
|
||||
self._replay_worker.statusReceived.connect(self._on_status_received)
|
||||
self._replay_worker.targetsUpdated.connect(self._on_radar_targets)
|
||||
self._replay_worker.statsUpdated.connect(self._on_radar_stats)
|
||||
self._replay_worker.errorOccurred.connect(self._on_worker_error)
|
||||
self._replay_worker.playbackStateChanged.connect(
|
||||
self._on_playback_state_changed)
|
||||
self._replay_worker.frameIndexChanged.connect(
|
||||
self._on_frame_index_changed)
|
||||
|
||||
# Start worker (paused initially)
|
||||
self._replay_worker.start()
|
||||
|
||||
# UI state
|
||||
self._running = True
|
||||
self._start_time = time.time()
|
||||
self._frame_count = 0
|
||||
self._start_btn.setEnabled(False)
|
||||
self._stop_btn.setEnabled(True)
|
||||
self._mode_combo.setEnabled(False)
|
||||
self._playback_frame.setVisible(True)
|
||||
self._pb_frame_label.setText(f"Frame: 0 / {info.n_frames}")
|
||||
self._pb_file_label.setText(
|
||||
f"{Path(npy_path).name} "
|
||||
f"({info.n_chirps}x{info.n_samples}, "
|
||||
f"{info.file_size_mb:.1f} MB)")
|
||||
self._status_label_main.setText("Status: Raw IQ Replay (paused)")
|
||||
self._sb_status.setText("Raw IQ Replay")
|
||||
self._sb_mode.setText("Raw IQ Replay")
|
||||
logger.info(f"Raw IQ Replay started: {npy_path}")
|
||||
|
||||
except (ValueError, OSError) as e:
|
||||
QMessageBox.critical(self, "Error",
|
||||
f"Failed to load raw IQ file:\n{e}")
|
||||
logger.error(f"Raw IQ load error: {e}")
|
||||
|
||||
# ---- Playback control slots --------------------------------------------
|
||||
|
||||
def _pb_play_pause(self):
|
||||
"""Toggle play/pause for raw IQ replay."""
|
||||
if self._replay_controller is None:
|
||||
return
|
||||
state = self._replay_controller.state
|
||||
if state == PlaybackState.PLAYING:
|
||||
self._replay_controller.pause()
|
||||
self._pb_play_btn.setText("Play")
|
||||
else:
|
||||
self._replay_controller.play()
|
||||
self._pb_play_btn.setText("Pause")
|
||||
|
||||
def _pb_step(self):
|
||||
"""Step one frame forward in raw IQ replay."""
|
||||
if self._replay_controller is not None:
|
||||
self._replay_controller.step_forward()
|
||||
|
||||
def _pb_fps_changed(self, value: float):
|
||||
if self._replay_controller is not None:
|
||||
self._replay_controller.set_fps(value)
|
||||
|
||||
def _pb_loop_changed(self, checked: bool):
|
||||
if self._replay_controller is not None:
|
||||
self._replay_controller.set_loop(checked)
|
||||
|
||||
@pyqtSlot(str)
|
||||
def _on_playback_state_changed(self, state_str: str):
|
||||
if state_str == "playing":
|
||||
self._pb_play_btn.setText("Pause")
|
||||
elif state_str == "paused":
|
||||
self._pb_play_btn.setText("Play")
|
||||
elif state_str == "stopped":
|
||||
self._pb_play_btn.setText("Play")
|
||||
self._status_label_main.setText("Status: Replay finished")
|
||||
|
||||
@pyqtSlot(int, int)
|
||||
def _on_frame_index_changed(self, current: int, total: int):
|
||||
self._pb_frame_label.setText(f"Frame: {current} / {total}")
|
||||
|
||||
# =====================================================================
|
||||
# Demo mode
|
||||
# =====================================================================
|
||||
@@ -1315,6 +1561,8 @@ class RadarDashboard(QMainWindow):
|
||||
self._demo_mode = False
|
||||
if not self._running:
|
||||
mode = "Idle"
|
||||
elif self._replay_controller is not None:
|
||||
mode = "Raw IQ Replay"
|
||||
elif isinstance(self._connection, ReplayConnection):
|
||||
mode = "Replay"
|
||||
else:
|
||||
@@ -1714,6 +1962,11 @@ class RadarDashboard(QMainWindow):
|
||||
def closeEvent(self, event):
|
||||
if self._simulator:
|
||||
self._simulator.stop()
|
||||
if self._replay_controller is not None:
|
||||
self._replay_controller.stop()
|
||||
if self._replay_worker is not None:
|
||||
self._replay_worker.stop()
|
||||
self._replay_worker.wait(1000)
|
||||
if self._radar_worker:
|
||||
self._radar_worker.stop()
|
||||
self._radar_worker.wait(1000)
|
||||
|
||||
@@ -2,9 +2,12 @@
|
||||
v7.processing — Radar signal processing and GPS parsing.
|
||||
|
||||
Classes:
|
||||
- RadarProcessor — dual-CPI fusion, multi-PRF unwrap, DBSCAN clustering,
|
||||
association, Kalman tracking
|
||||
- USBPacketParser — parse GPS text/binary frames from STM32 CDC
|
||||
- RadarProcessor — dual-CPI fusion, multi-PRF unwrap, DBSCAN clustering,
|
||||
association, Kalman tracking
|
||||
- RawIQFrameProcessor — full signal chain for raw IQ replay:
|
||||
quantize -> AGC -> Range FFT -> Doppler FFT ->
|
||||
crop -> MTI -> DC notch -> mag -> CFAR
|
||||
- USBPacketParser — parse GPS text/binary frames from STM32 CDC
|
||||
|
||||
Note: RadarPacketParser (old A5/C3 sync + CRC16 format) was removed.
|
||||
All packet parsing now uses production RadarProtocol (0xAA/0xBB format)
|
||||
@@ -22,6 +25,11 @@ from .models import (
|
||||
RadarTarget, GPSData, ProcessingConfig,
|
||||
SCIPY_AVAILABLE, SKLEARN_AVAILABLE, FILTERPY_AVAILABLE,
|
||||
)
|
||||
from .agc_sim import (
|
||||
AGCConfig, AGCState, AGCFrameResult,
|
||||
quantize_iq, process_agc_frame,
|
||||
)
|
||||
from .hardware import RadarFrame, StatusResponse
|
||||
|
||||
if SKLEARN_AVAILABLE:
|
||||
from sklearn.cluster import DBSCAN
|
||||
@@ -48,6 +56,103 @@ def apply_pitch_correction(raw_elevation: float, pitch: float) -> float:
|
||||
return raw_elevation - pitch
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Utility: bin-to-physical target extraction (shared by all workers)
|
||||
# =============================================================================
|
||||
|
||||
def extract_targets_from_frame(
|
||||
frame: RadarFrame,
|
||||
range_resolution: float,
|
||||
velocity_resolution: float,
|
||||
*,
|
||||
gps: GPSData | None = None,
|
||||
) -> list[RadarTarget]:
|
||||
"""Extract RadarTargets from a RadarFrame's detection mask.
|
||||
|
||||
Performs bin-to-physical conversion and optional GPS coordinate mapping.
|
||||
This is the shared implementation used by both RadarDataWorker (live mode)
|
||||
and RawIQReplayWorker (replay mode).
|
||||
|
||||
Args:
|
||||
frame: RadarFrame with populated ``detections`` and ``magnitude`` arrays.
|
||||
range_resolution: Metres per range bin.
|
||||
velocity_resolution: m/s per Doppler bin.
|
||||
gps: Optional GPSData for pitch correction and geographic mapping.
|
||||
|
||||
Returns:
|
||||
List of RadarTarget with physical-unit range, velocity, SNR, and
|
||||
(if GPS available) lat/lon/azimuth/elevation.
|
||||
"""
|
||||
det_indices = np.argwhere(frame.detections > 0)
|
||||
if len(det_indices) == 0:
|
||||
return []
|
||||
|
||||
n_doppler = frame.magnitude.shape[1]
|
||||
center_dbin = n_doppler // 2
|
||||
targets: list[RadarTarget] = []
|
||||
|
||||
for idx in det_indices:
|
||||
rbin, dbin = idx
|
||||
mag = frame.magnitude[rbin, dbin]
|
||||
snr = 10 * np.log10(max(mag, 1)) if mag > 0 else 0.0
|
||||
|
||||
range_m = float(rbin) * range_resolution
|
||||
velocity_ms = float(dbin - center_dbin) * velocity_resolution
|
||||
|
||||
# GPS-dependent fields
|
||||
raw_elev = 0.0
|
||||
corr_elev = raw_elev
|
||||
lat, lon, azimuth = 0.0, 0.0, 0.0
|
||||
if gps is not None:
|
||||
corr_elev = apply_pitch_correction(raw_elev, gps.pitch)
|
||||
azimuth = gps.heading
|
||||
lat, lon = _polar_to_geographic(
|
||||
gps.latitude, gps.longitude, range_m, azimuth)
|
||||
|
||||
targets.append(RadarTarget(
|
||||
id=len(targets),
|
||||
range=range_m,
|
||||
velocity=velocity_ms,
|
||||
azimuth=azimuth,
|
||||
elevation=corr_elev,
|
||||
latitude=lat,
|
||||
longitude=lon,
|
||||
snr=snr,
|
||||
timestamp=frame.timestamp,
|
||||
))
|
||||
|
||||
return targets
|
||||
|
||||
|
||||
def _polar_to_geographic(
|
||||
radar_lat: float, radar_lon: float, range_m: float, bearing_deg: float,
|
||||
) -> tuple[float, float]:
|
||||
"""Convert polar (range, bearing) to geographic (lat, lon).
|
||||
|
||||
Uses the spherical-Earth approximation (adequate for <50 km ranges).
|
||||
Duplicated from ``workers.polar_to_geographic`` to keep processing.py
|
||||
self-contained; the workers module still exports its own copy for
|
||||
backward-compat.
|
||||
"""
|
||||
if range_m <= 0:
|
||||
return radar_lat, radar_lon
|
||||
earth_r = 6_371_000.0
|
||||
lat_r = math.radians(radar_lat)
|
||||
lon_r = math.radians(radar_lon)
|
||||
brg_r = math.radians(bearing_deg)
|
||||
d_r = range_m / earth_r
|
||||
|
||||
new_lat = math.asin(
|
||||
math.sin(lat_r) * math.cos(d_r)
|
||||
+ math.cos(lat_r) * math.sin(d_r) * math.cos(brg_r)
|
||||
)
|
||||
new_lon = lon_r + math.atan2(
|
||||
math.sin(brg_r) * math.sin(d_r) * math.cos(lat_r),
|
||||
math.cos(d_r) - math.sin(lat_r) * math.sin(new_lat),
|
||||
)
|
||||
return math.degrees(new_lat), math.degrees(new_lon)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Radar Processor — signal-level processing & tracking pipeline
|
||||
# =============================================================================
|
||||
@@ -451,3 +556,269 @@ class USBPacketParser:
|
||||
except (ValueError, struct.error) as e:
|
||||
logger.error(f"Error parsing binary GPS: {e}")
|
||||
return None
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Raw IQ Frame Processor — full signal chain for replay mode
|
||||
# =============================================================================
|
||||
|
||||
class RawIQFrameProcessor:
|
||||
"""Process raw complex IQ frames through the full radar signal chain.
|
||||
|
||||
This replicates the FPGA processing pipeline in software so that
|
||||
raw ADI CN0566 captures (or similar) can be visualised in the V7
|
||||
dashboard exactly as they would appear from the FPGA.
|
||||
|
||||
Pipeline per frame:
|
||||
1. Quantize raw complex → 16-bit signed I/Q
|
||||
2. AGC gain application (bit-accurate to rx_gain_control.v)
|
||||
3. Range FFT (across samples)
|
||||
4. Doppler FFT (across chirps) + fftshift + centre crop
|
||||
5. Optional MTI (2-pulse canceller using history)
|
||||
6. Optional DC notch (zero-Doppler removal)
|
||||
7. Magnitude (|I| + |Q| approximation matching FPGA, or true |.|)
|
||||
8. CFAR or simple threshold detection
|
||||
9. Build RadarFrame + synthetic StatusResponse
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
n_range_out: int = 64,
|
||||
n_doppler_out: int = 32,
|
||||
):
|
||||
self._n_range = n_range_out
|
||||
self._n_doppler = n_doppler_out
|
||||
|
||||
# AGC state (persists across frames)
|
||||
self._agc_config = AGCConfig()
|
||||
self._agc_state = AGCState()
|
||||
|
||||
# MTI history buffer (stores previous Range-Doppler maps)
|
||||
self._mti_history: list[np.ndarray] = []
|
||||
self._mti_enabled: bool = False
|
||||
|
||||
# DC notch
|
||||
self._dc_notch_width: int = 0
|
||||
|
||||
# CFAR / threshold config
|
||||
self._cfar_enabled: bool = False
|
||||
self._cfar_guard: int = 2
|
||||
self._cfar_train: int = 8
|
||||
self._cfar_alpha_q44: int = 0x30 # Q4.4 → 3.0
|
||||
self._cfar_mode: int = 0 # 0=CA, 1=GO, 2=SO
|
||||
self._detect_threshold: int = 10000
|
||||
|
||||
# Frame counter
|
||||
self._frame_number: int = 0
|
||||
|
||||
# Host-side processing (windowing, clustering, etc.)
|
||||
self._host_processor = RadarProcessor()
|
||||
|
||||
# ---- Configuration setters ---------------------------------------------
|
||||
|
||||
def set_agc_config(self, config: AGCConfig) -> None:
|
||||
self._agc_config = config
|
||||
|
||||
def reset_agc_state(self) -> None:
|
||||
"""Reset AGC state (e.g. on seek)."""
|
||||
self._agc_state = AGCState()
|
||||
self._mti_history.clear()
|
||||
|
||||
def set_mti_enabled(self, enabled: bool) -> None:
|
||||
if self._mti_enabled != enabled:
|
||||
self._mti_history.clear()
|
||||
self._mti_enabled = enabled
|
||||
|
||||
def set_dc_notch_width(self, width: int) -> None:
|
||||
self._dc_notch_width = max(0, min(7, width))
|
||||
|
||||
def set_cfar_params(
|
||||
self,
|
||||
enabled: bool,
|
||||
guard: int = 2,
|
||||
train: int = 8,
|
||||
alpha_q44: int = 0x30,
|
||||
mode: int = 0,
|
||||
) -> None:
|
||||
self._cfar_enabled = enabled
|
||||
self._cfar_guard = guard
|
||||
self._cfar_train = train
|
||||
self._cfar_alpha_q44 = alpha_q44
|
||||
self._cfar_mode = mode
|
||||
|
||||
def set_detect_threshold(self, threshold: int) -> None:
|
||||
self._detect_threshold = threshold
|
||||
|
||||
@property
|
||||
def agc_state(self) -> AGCState:
|
||||
return self._agc_state
|
||||
|
||||
@property
|
||||
def agc_config(self) -> AGCConfig:
|
||||
return self._agc_config
|
||||
|
||||
@property
|
||||
def frame_number(self) -> int:
|
||||
return self._frame_number
|
||||
|
||||
# ---- Main processing entry point ---------------------------------------
|
||||
|
||||
def process_frame(
|
||||
self,
|
||||
raw_frame: np.ndarray,
|
||||
timestamp: float = 0.0,
|
||||
) -> tuple[RadarFrame, StatusResponse, AGCFrameResult]:
|
||||
"""Process one raw IQ frame through the full chain.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
raw_frame : complex array, shape (n_chirps, n_samples)
|
||||
timestamp : frame timestamp for RadarFrame
|
||||
|
||||
Returns
|
||||
-------
|
||||
(radar_frame, status_response, agc_result)
|
||||
"""
|
||||
n_chirps, _n_samples = raw_frame.shape
|
||||
self._frame_number += 1
|
||||
|
||||
# 1. Quantize to 16-bit signed IQ
|
||||
frame_i, frame_q = quantize_iq(raw_frame)
|
||||
|
||||
# 2. AGC
|
||||
agc_result = process_agc_frame(
|
||||
frame_i, frame_q, self._agc_config, self._agc_state)
|
||||
|
||||
# Use AGC-shifted IQ for downstream processing
|
||||
iq = agc_result.shifted_i.astype(np.float64) + 1j * agc_result.shifted_q.astype(np.float64)
|
||||
|
||||
# 3. Range FFT (across samples axis)
|
||||
range_fft = np.fft.fft(iq, axis=1)[:, :self._n_range]
|
||||
|
||||
# 4. Doppler FFT (across chirps axis) + fftshift + centre crop
|
||||
doppler_fft = np.fft.fft(range_fft, axis=0)
|
||||
doppler_fft = np.fft.fftshift(doppler_fft, axes=0)
|
||||
# Centre-crop to n_doppler bins
|
||||
center = n_chirps // 2
|
||||
half_d = self._n_doppler // 2
|
||||
start_d = max(0, center - half_d)
|
||||
end_d = start_d + self._n_doppler
|
||||
if end_d > n_chirps:
|
||||
end_d = n_chirps
|
||||
start_d = max(0, end_d - self._n_doppler)
|
||||
rd_complex = doppler_fft[start_d:end_d, :]
|
||||
# shape: (n_doppler, n_range) → transpose to (n_range, n_doppler)
|
||||
rd_complex = rd_complex.T
|
||||
|
||||
# 5. Optional MTI (2-pulse canceller)
|
||||
if self._mti_enabled:
|
||||
rd_complex = self._apply_mti(rd_complex)
|
||||
|
||||
# 6. Optional DC notch (zero-Doppler bins)
|
||||
if self._dc_notch_width > 0:
|
||||
rd_complex = self._apply_dc_notch(rd_complex)
|
||||
|
||||
# Extract I/Q for RadarFrame
|
||||
rd_i = np.round(rd_complex.real).astype(np.int16)
|
||||
rd_q = np.round(rd_complex.imag).astype(np.int16)
|
||||
|
||||
# 7. Magnitude (FPGA uses |I|+|Q| approximation)
|
||||
magnitude = np.abs(rd_complex.real) + np.abs(rd_complex.imag)
|
||||
|
||||
# Range profile (sum across Doppler)
|
||||
range_profile = np.sum(magnitude, axis=1)
|
||||
|
||||
# 8. Detection (CFAR or simple threshold)
|
||||
if self._cfar_enabled:
|
||||
detections = self._run_cfar(magnitude)
|
||||
else:
|
||||
detections = self._run_threshold(magnitude)
|
||||
|
||||
detection_count = int(np.sum(detections > 0))
|
||||
|
||||
# 9. Build RadarFrame
|
||||
radar_frame = RadarFrame(
|
||||
timestamp=timestamp,
|
||||
range_doppler_i=rd_i,
|
||||
range_doppler_q=rd_q,
|
||||
magnitude=magnitude,
|
||||
detections=detections,
|
||||
range_profile=range_profile,
|
||||
detection_count=detection_count,
|
||||
frame_number=self._frame_number,
|
||||
)
|
||||
|
||||
# 10. Build synthetic StatusResponse
|
||||
status = self._build_status(agc_result)
|
||||
|
||||
return radar_frame, status, agc_result
|
||||
|
||||
# ---- Internal helpers --------------------------------------------------
|
||||
|
||||
def _apply_mti(self, rd: np.ndarray) -> np.ndarray:
|
||||
"""2-pulse MTI canceller: y[n] = x[n] - x[n-1]."""
|
||||
self._mti_history.append(rd.copy())
|
||||
if len(self._mti_history) > 2:
|
||||
self._mti_history = self._mti_history[-2:]
|
||||
|
||||
if len(self._mti_history) < 2:
|
||||
return np.zeros_like(rd) # suppress first frame
|
||||
|
||||
return self._mti_history[-1] - self._mti_history[-2]
|
||||
|
||||
def _apply_dc_notch(self, rd: np.ndarray) -> np.ndarray:
|
||||
"""Zero out centre Doppler bins (DC notch)."""
|
||||
n_doppler = rd.shape[1]
|
||||
center = n_doppler // 2
|
||||
w = self._dc_notch_width
|
||||
lo = max(0, center - w)
|
||||
hi = min(n_doppler, center + w + 1)
|
||||
result = rd.copy()
|
||||
result[:, lo:hi] = 0
|
||||
return result
|
||||
|
||||
def _run_cfar(self, magnitude: np.ndarray) -> np.ndarray:
|
||||
"""Run 1-D CFAR along each range bin (Doppler direction).
|
||||
|
||||
Uses the host-side CFAR from RadarProcessor with alpha converted
|
||||
from Q4.4 to float.
|
||||
"""
|
||||
alpha_float = self._cfar_alpha_q44 / 16.0
|
||||
cfar_types = {0: "CA-CFAR", 1: "GO-CFAR", 2: "SO-CFAR"}
|
||||
cfar_type = cfar_types.get(self._cfar_mode, "CA-CFAR")
|
||||
|
||||
power = magnitude ** 2
|
||||
power = np.maximum(power, 1e-20)
|
||||
|
||||
mask = np.zeros_like(magnitude, dtype=np.uint8)
|
||||
for r in range(magnitude.shape[0]):
|
||||
row = power[r, :]
|
||||
if row.max() > 0:
|
||||
det = RadarProcessor.cfar_1d(
|
||||
row, self._cfar_guard, self._cfar_train,
|
||||
alpha_float, cfar_type)
|
||||
mask[r, :] = det.astype(np.uint8)
|
||||
return mask
|
||||
|
||||
def _run_threshold(self, magnitude: np.ndarray) -> np.ndarray:
|
||||
"""Simple threshold detection (matches FPGA detect_threshold)."""
|
||||
return (magnitude > self._detect_threshold).astype(np.uint8)
|
||||
|
||||
def _build_status(self, agc_result: AGCFrameResult) -> StatusResponse:
|
||||
"""Build a synthetic StatusResponse from current processor state."""
|
||||
return StatusResponse(
|
||||
radar_mode=1, # active
|
||||
stream_ctrl=0b111,
|
||||
cfar_threshold=self._detect_threshold,
|
||||
long_chirp=3000,
|
||||
long_listen=13700,
|
||||
guard=17540,
|
||||
short_chirp=50,
|
||||
short_listen=17450,
|
||||
chirps_per_elev=32,
|
||||
range_mode=0,
|
||||
agc_current_gain=agc_result.gain_enc,
|
||||
agc_peak_magnitude=agc_result.peak_mag_8bit,
|
||||
agc_saturation_count=agc_result.saturation_count,
|
||||
agc_enable=1 if self._agc_config.enabled else 0,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,264 @@
|
||||
"""
|
||||
v7.raw_iq_replay -- Raw IQ replay controller for the V7 dashboard.
|
||||
|
||||
Manages loading of raw complex IQ .npy captures, playback state
|
||||
(play/pause/step/speed/loop), and delivers frames to a worker thread.
|
||||
|
||||
The controller is thread-safe: the worker calls ``next_frame()`` which
|
||||
blocks until a frame is available or playback is stopped.
|
||||
|
||||
Supported file formats:
|
||||
- 3-D .npy: (n_frames, n_chirps, n_samples) complex
|
||||
- 2-D .npy: (n_chirps, n_samples) complex -> treated as single frame
|
||||
|
||||
Classes:
|
||||
- RawIQReplayController -- playback state machine + frame delivery
|
||||
- RawIQFileInfo -- metadata about the loaded file
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Playback state enum
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class PlaybackState(Enum):
|
||||
"""Playback state for the replay controller."""
|
||||
STOPPED = auto()
|
||||
PLAYING = auto()
|
||||
PAUSED = auto()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# File metadata
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@dataclass
|
||||
class RawIQFileInfo:
|
||||
"""Metadata about a loaded raw IQ .npy file."""
|
||||
path: str
|
||||
n_frames: int
|
||||
n_chirps: int
|
||||
n_samples: int
|
||||
dtype: str
|
||||
file_size_mb: float
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Replay Controller
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class RawIQReplayController:
|
||||
"""Manages raw IQ file loading and playback state.
|
||||
|
||||
Thread-safety: the controller uses a condition variable so the worker
|
||||
thread can block on ``next_frame()`` waiting for play/step events,
|
||||
while the GUI thread calls ``play()``, ``pause()``, ``step()``, etc.
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._data: np.ndarray | None = None
|
||||
self._info: RawIQFileInfo | None = None
|
||||
|
||||
# Playback state
|
||||
self._state = PlaybackState.STOPPED
|
||||
self._frame_index: int = 0
|
||||
self._fps: float = 10.0 # target frames per second
|
||||
self._loop: bool = True
|
||||
|
||||
# Thread synchronisation
|
||||
self._lock = threading.Lock()
|
||||
self._cond = threading.Condition(self._lock)
|
||||
|
||||
# Step request flag (set by GUI, consumed by worker)
|
||||
self._step_requested: bool = False
|
||||
|
||||
# Stop signal
|
||||
self._stop_requested: bool = False
|
||||
|
||||
# ---- File loading ------------------------------------------------------
|
||||
|
||||
def load_file(self, path: str) -> RawIQFileInfo:
|
||||
"""Load a .npy file containing raw IQ data.
|
||||
|
||||
Raises ValueError if the file is not a valid raw IQ capture.
|
||||
"""
|
||||
p = Path(path)
|
||||
if not p.exists():
|
||||
msg = f"File not found: {path}"
|
||||
raise ValueError(msg)
|
||||
if p.suffix.lower() != ".npy":
|
||||
msg = f"Expected .npy file, got: {p.suffix}"
|
||||
raise ValueError(msg)
|
||||
|
||||
# Memory-map for large files
|
||||
data = np.load(str(p), mmap_mode="r")
|
||||
|
||||
if not np.iscomplexobj(data):
|
||||
msg = f"Expected complex data, got dtype={data.dtype}"
|
||||
raise ValueError(msg)
|
||||
|
||||
# Normalise shape
|
||||
if data.ndim == 2:
|
||||
# Single frame: (chirps, samples) -> (1, chirps, samples)
|
||||
data = data[np.newaxis, :, :]
|
||||
elif data.ndim == 3:
|
||||
pass # (frames, chirps, samples) — expected
|
||||
else:
|
||||
msg = f"Expected 2-D or 3-D array, got {data.ndim}-D"
|
||||
raise ValueError(msg)
|
||||
|
||||
with self._lock:
|
||||
self._data = data
|
||||
self._frame_index = 0
|
||||
self._state = PlaybackState.PAUSED
|
||||
self._stop_requested = False
|
||||
|
||||
self._info = RawIQFileInfo(
|
||||
path=str(p),
|
||||
n_frames=data.shape[0],
|
||||
n_chirps=data.shape[1],
|
||||
n_samples=data.shape[2],
|
||||
dtype=str(data.dtype),
|
||||
file_size_mb=p.stat().st_size / (1024 * 1024),
|
||||
)
|
||||
|
||||
logger.info(
|
||||
f"Loaded raw IQ: {p.name} — {self._info.n_frames} frames, "
|
||||
f"{self._info.n_chirps} chirps, {self._info.n_samples} samples, "
|
||||
f"{self._info.file_size_mb:.1f} MB"
|
||||
)
|
||||
return self._info
|
||||
|
||||
def unload(self) -> None:
|
||||
"""Unload the current file and stop playback."""
|
||||
with self._lock:
|
||||
self._state = PlaybackState.STOPPED
|
||||
self._stop_requested = True
|
||||
self._data = None
|
||||
self._info = None
|
||||
self._frame_index = 0
|
||||
self._cond.notify_all()
|
||||
|
||||
# ---- Playback control (called from GUI thread) -------------------------
|
||||
|
||||
def play(self) -> None:
|
||||
with self._lock:
|
||||
if self._data is None:
|
||||
return
|
||||
self._state = PlaybackState.PLAYING
|
||||
self._stop_requested = False
|
||||
self._cond.notify_all()
|
||||
|
||||
def pause(self) -> None:
|
||||
with self._lock:
|
||||
if self._state == PlaybackState.PLAYING:
|
||||
self._state = PlaybackState.PAUSED
|
||||
|
||||
def stop(self) -> None:
|
||||
with self._lock:
|
||||
self._state = PlaybackState.STOPPED
|
||||
self._stop_requested = True
|
||||
self._cond.notify_all()
|
||||
|
||||
def step_forward(self) -> None:
|
||||
"""Advance one frame (works in PAUSED state)."""
|
||||
with self._lock:
|
||||
if self._data is None:
|
||||
return
|
||||
self._step_requested = True
|
||||
self._cond.notify_all()
|
||||
|
||||
def seek(self, frame_index: int) -> None:
|
||||
"""Jump to a specific frame index."""
|
||||
with self._lock:
|
||||
if self._data is None:
|
||||
return
|
||||
self._frame_index = max(0, min(frame_index, self._data.shape[0] - 1))
|
||||
|
||||
def set_fps(self, fps: float) -> None:
|
||||
with self._lock:
|
||||
self._fps = max(0.1, min(60.0, fps))
|
||||
|
||||
def set_loop(self, loop: bool) -> None:
|
||||
with self._lock:
|
||||
self._loop = loop
|
||||
|
||||
# ---- State queries (thread-safe) ---------------------------------------
|
||||
|
||||
@property
|
||||
def state(self) -> PlaybackState:
|
||||
with self._lock:
|
||||
return self._state
|
||||
|
||||
@property
|
||||
def frame_index(self) -> int:
|
||||
with self._lock:
|
||||
return self._frame_index
|
||||
|
||||
@property
|
||||
def info(self) -> RawIQFileInfo | None:
|
||||
with self._lock:
|
||||
return self._info
|
||||
|
||||
@property
|
||||
def fps(self) -> float:
|
||||
with self._lock:
|
||||
return self._fps
|
||||
|
||||
@property
|
||||
def is_loaded(self) -> bool:
|
||||
with self._lock:
|
||||
return self._data is not None
|
||||
|
||||
# ---- Frame delivery (called from worker thread) ------------------------
|
||||
|
||||
def next_frame(self) -> np.ndarray | None:
|
||||
"""Block until the next frame is available, then return it.
|
||||
|
||||
Returns None when playback is stopped or file is unloaded.
|
||||
The caller (worker thread) should use this in a loop.
|
||||
"""
|
||||
with self._cond:
|
||||
while True:
|
||||
if self._stop_requested or self._data is None:
|
||||
return None
|
||||
|
||||
if self._state == PlaybackState.PLAYING:
|
||||
return self._deliver_frame()
|
||||
|
||||
if self._step_requested:
|
||||
self._step_requested = False
|
||||
return self._deliver_frame()
|
||||
|
||||
# PAUSED or STOPPED — wait for signal
|
||||
self._cond.wait(timeout=0.1)
|
||||
|
||||
def _deliver_frame(self) -> np.ndarray | None:
|
||||
"""Return current frame and advance index. Must hold lock."""
|
||||
if self._data is None:
|
||||
return None
|
||||
|
||||
n_frames = self._data.shape[0]
|
||||
if self._frame_index >= n_frames:
|
||||
if self._loop:
|
||||
self._frame_index = 0
|
||||
else:
|
||||
self._state = PlaybackState.PAUSED
|
||||
return None
|
||||
|
||||
# Read the frame (memory-mapped, so this is cheap)
|
||||
frame = np.array(self._data[self._frame_index]) # copy from mmap
|
||||
self._frame_index += 1
|
||||
return frame
|
||||
@@ -2,11 +2,14 @@
|
||||
v7.workers — QThread-based workers and demo target simulator.
|
||||
|
||||
Classes:
|
||||
- RadarDataWorker — reads from FT2232H via production RadarAcquisition,
|
||||
parses 0xAA/0xBB packets, assembles 64x32 frames,
|
||||
runs host-side DSP, emits PyQt signals.
|
||||
- GPSDataWorker — reads GPS frames from STM32 CDC, emits GPSData signals.
|
||||
- TargetSimulator — QTimer-based demo target generator.
|
||||
- RadarDataWorker — reads from FT2232H via production RadarAcquisition,
|
||||
parses 0xAA/0xBB packets, assembles 64x32 frames,
|
||||
runs host-side DSP, emits PyQt signals.
|
||||
- RawIQReplayWorker — reads raw IQ .npy frames from RawIQReplayController,
|
||||
processes through RawIQFrameProcessor, emits same
|
||||
signals as RadarDataWorker + playback state.
|
||||
- GPSDataWorker — reads GPS frames from STM32 CDC, emits GPSData signals.
|
||||
- TargetSimulator — QTimer-based demo target generator.
|
||||
|
||||
The old V6/V7 packet parsing (sync A5 C3 + type + CRC16) has been removed.
|
||||
All packet parsing now uses the production radar_protocol.py which matches
|
||||
@@ -20,8 +23,6 @@ import queue
|
||||
import struct
|
||||
import logging
|
||||
|
||||
import numpy as np
|
||||
|
||||
from PyQt6.QtCore import QThread, QObject, QTimer, pyqtSignal
|
||||
|
||||
from .models import RadarTarget, GPSData, RadarSettings
|
||||
@@ -34,9 +35,11 @@ from .hardware import (
|
||||
)
|
||||
from .processing import (
|
||||
RadarProcessor,
|
||||
RawIQFrameProcessor,
|
||||
USBPacketParser,
|
||||
apply_pitch_correction,
|
||||
extract_targets_from_frame,
|
||||
)
|
||||
from .raw_iq_replay import RawIQReplayController, PlaybackState
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@@ -206,55 +209,16 @@ class RadarDataWorker(QThread):
|
||||
Bin-to-physical conversion uses RadarSettings.range_resolution
|
||||
and velocity_resolution (should be calibrated to actual waveform).
|
||||
"""
|
||||
targets: list[RadarTarget] = []
|
||||
|
||||
cfg = self._processor.config
|
||||
if not (cfg.clustering_enabled or cfg.tracking_enabled):
|
||||
return targets
|
||||
return []
|
||||
|
||||
# Extract detections from FPGA CFAR flags
|
||||
det_indices = np.argwhere(frame.detections > 0)
|
||||
r_res = self._settings.range_resolution
|
||||
v_res = self._settings.velocity_resolution
|
||||
|
||||
for idx in det_indices:
|
||||
rbin, dbin = idx
|
||||
mag = frame.magnitude[rbin, dbin]
|
||||
snr = 10 * np.log10(max(mag, 1)) if mag > 0 else 0
|
||||
|
||||
# Convert bin indices to physical units
|
||||
range_m = float(rbin) * r_res
|
||||
# Doppler: centre bin (16) = 0 m/s; positive bins = approaching
|
||||
velocity_ms = float(dbin - 16) * v_res
|
||||
|
||||
# Apply pitch correction if GPS data available
|
||||
raw_elev = 0.0 # FPGA doesn't send elevation per-detection
|
||||
corr_elev = raw_elev
|
||||
if self._gps:
|
||||
corr_elev = apply_pitch_correction(raw_elev, self._gps.pitch)
|
||||
|
||||
# Compute geographic position if GPS available
|
||||
lat, lon = 0.0, 0.0
|
||||
azimuth = 0.0 # No azimuth from single-beam; set to heading
|
||||
if self._gps:
|
||||
azimuth = self._gps.heading
|
||||
lat, lon = polar_to_geographic(
|
||||
self._gps.latitude, self._gps.longitude,
|
||||
range_m, azimuth,
|
||||
)
|
||||
|
||||
target = RadarTarget(
|
||||
id=len(targets),
|
||||
range=range_m,
|
||||
velocity=velocity_ms,
|
||||
azimuth=azimuth,
|
||||
elevation=corr_elev,
|
||||
latitude=lat,
|
||||
longitude=lon,
|
||||
snr=snr,
|
||||
timestamp=frame.timestamp,
|
||||
)
|
||||
targets.append(target)
|
||||
targets = extract_targets_from_frame(
|
||||
frame,
|
||||
self._settings.range_resolution,
|
||||
self._settings.velocity_resolution,
|
||||
gps=self._gps,
|
||||
)
|
||||
|
||||
# DBSCAN clustering
|
||||
if cfg.clustering_enabled and len(targets) > 0:
|
||||
@@ -268,6 +232,147 @@ class RadarDataWorker(QThread):
|
||||
return targets
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Raw IQ Replay Worker (QThread) — processes raw .npy captures
|
||||
# =============================================================================
|
||||
|
||||
class RawIQReplayWorker(QThread):
|
||||
"""Background worker for raw IQ replay mode.
|
||||
|
||||
Reads frames from a RawIQReplayController, processes them through
|
||||
RawIQFrameProcessor (quantize -> AGC -> FFT -> CFAR -> RadarFrame),
|
||||
and emits the same signals as RadarDataWorker so the dashboard can
|
||||
display them identically.
|
||||
|
||||
Additional signal:
|
||||
playbackStateChanged(str) — "playing", "paused", "stopped"
|
||||
frameIndexChanged(int, int) — (current_index, total_frames)
|
||||
|
||||
Signals:
|
||||
frameReady(RadarFrame)
|
||||
statusReceived(object)
|
||||
targetsUpdated(list)
|
||||
errorOccurred(str)
|
||||
statsUpdated(dict)
|
||||
playbackStateChanged(str)
|
||||
frameIndexChanged(int, int)
|
||||
"""
|
||||
|
||||
frameReady = pyqtSignal(object)
|
||||
statusReceived = pyqtSignal(object)
|
||||
targetsUpdated = pyqtSignal(list)
|
||||
errorOccurred = pyqtSignal(str)
|
||||
statsUpdated = pyqtSignal(dict)
|
||||
playbackStateChanged = pyqtSignal(str)
|
||||
frameIndexChanged = pyqtSignal(int, int)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
controller: RawIQReplayController,
|
||||
processor: RawIQFrameProcessor,
|
||||
host_processor: RadarProcessor | None = None,
|
||||
settings: RadarSettings | None = None,
|
||||
parent=None,
|
||||
):
|
||||
super().__init__(parent)
|
||||
self._controller = controller
|
||||
self._processor = processor
|
||||
self._host_processor = host_processor
|
||||
self._settings = settings or RadarSettings()
|
||||
self._running = False
|
||||
self._frame_count = 0
|
||||
self._error_count = 0
|
||||
|
||||
def stop(self):
|
||||
self._running = False
|
||||
self._controller.stop()
|
||||
|
||||
def run(self):
|
||||
self._running = True
|
||||
self._frame_count = 0
|
||||
logger.info("RawIQReplayWorker started")
|
||||
|
||||
info = self._controller.info
|
||||
total_frames = info.n_frames if info else 0
|
||||
|
||||
while self._running:
|
||||
try:
|
||||
# Block until next frame or stop
|
||||
raw_frame = self._controller.next_frame()
|
||||
if raw_frame is None:
|
||||
# Stopped or end of file
|
||||
if self._running:
|
||||
self.playbackStateChanged.emit("stopped")
|
||||
break
|
||||
|
||||
# Process through full signal chain
|
||||
import time as _time
|
||||
ts = _time.time()
|
||||
frame, status, _agc_result = self._processor.process_frame(
|
||||
raw_frame, timestamp=ts)
|
||||
self._frame_count += 1
|
||||
|
||||
# Emit signals
|
||||
self.frameReady.emit(frame)
|
||||
self.statusReceived.emit(status)
|
||||
|
||||
# Emit frame index
|
||||
idx = self._controller.frame_index
|
||||
self.frameIndexChanged.emit(idx, total_frames)
|
||||
|
||||
# Emit playback state
|
||||
state = self._controller.state
|
||||
self.playbackStateChanged.emit(state.name.lower())
|
||||
|
||||
# Run host-side DSP if configured
|
||||
if self._host_processor is not None:
|
||||
targets = self._extract_targets(frame)
|
||||
if targets:
|
||||
self.targetsUpdated.emit(targets)
|
||||
|
||||
# Stats
|
||||
self.statsUpdated.emit({
|
||||
"frames": self._frame_count,
|
||||
"detection_count": frame.detection_count,
|
||||
"errors": self._error_count,
|
||||
"frame_index": idx,
|
||||
"total_frames": total_frames,
|
||||
})
|
||||
|
||||
# Rate limiting: sleep to match target FPS
|
||||
fps = self._controller.fps
|
||||
if fps > 0 and self._controller.state == PlaybackState.PLAYING:
|
||||
self.msleep(int(1000.0 / fps))
|
||||
|
||||
except (ValueError, IndexError) as e:
|
||||
self._error_count += 1
|
||||
self.errorOccurred.emit(str(e))
|
||||
logger.error(f"RawIQReplayWorker error: {e}")
|
||||
|
||||
self._running = False
|
||||
logger.info("RawIQReplayWorker stopped")
|
||||
|
||||
def _extract_targets(self, frame: RadarFrame) -> list[RadarTarget]:
|
||||
"""Extract targets from detection mask using shared bin-to-physical conversion."""
|
||||
targets = extract_targets_from_frame(
|
||||
frame,
|
||||
self._settings.range_resolution,
|
||||
self._settings.velocity_resolution,
|
||||
)
|
||||
|
||||
# Clustering + tracking
|
||||
if self._host_processor is not None:
|
||||
cfg = self._host_processor.config
|
||||
if cfg.clustering_enabled and len(targets) > 0:
|
||||
clusters = self._host_processor.clustering(
|
||||
targets, cfg.clustering_eps, cfg.clustering_min_samples)
|
||||
if cfg.tracking_enabled:
|
||||
targets = self._host_processor.association(targets, clusters)
|
||||
self._host_processor.tracking(targets)
|
||||
|
||||
return targets
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# GPS Data Worker (QThread)
|
||||
# =============================================================================
|
||||
|
||||
Reference in New Issue
Block a user