feat: unified replay with SoftwareFPGA bit-accurate signal chain
Add SoftwareFPGA class that imports golden_reference functions to replicate the FPGA pipeline in software, enabling bit-accurate replay of raw IQ, FPGA co-sim, and HDF5 recordings through the same dashboard path as live data. New modules: software_fpga.py, replay.py (ReplayEngine + 3 loaders) Enhanced: WaveformConfig model, extract_targets_from_frame() in processing, ReplayWorker with thread-safe playback controls, dashboard replay UI with transport controls and dual-dispatch FPGA parameter routing. Removed: ReplayConnection (from radar_protocol, hardware, dashboard, tests) — replaced by the unified replay architecture. 150/150 tests pass, ruff clean.
This commit is contained in:
Jason
@@ -15,7 +15,6 @@ USB Packet Protocol (11-byte):
|
||||
Command: 4 bytes received sequentially {opcode, addr, value_hi, value_lo}
|
||||
"""
|
||||
|
||||
import os
|
||||
import struct
|
||||
import time
|
||||
import threading
|
||||
@@ -443,391 +442,7 @@ class FT2232HConnection:
|
||||
return bytes(buf)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# Replay Connection — feed real .npy data through the dashboard
|
||||
# ============================================================================
|
||||
|
||||
# Hardware-only opcodes that cannot be adjusted in replay mode
|
||||
# Values must match radar_system_top.v case(usb_cmd_opcode).
|
||||
_HARDWARE_ONLY_OPCODES = {
|
||||
0x01, # RADAR_MODE
|
||||
0x02, # TRIGGER_PULSE
|
||||
# 0x03 (DETECT_THRESHOLD) is NOT hardware-only — it's in _REPLAY_ADJUSTABLE_OPCODES
|
||||
0x04, # STREAM_CONTROL
|
||||
0x10, # LONG_CHIRP
|
||||
0x11, # LONG_LISTEN
|
||||
0x12, # GUARD
|
||||
0x13, # SHORT_CHIRP
|
||||
0x14, # SHORT_LISTEN
|
||||
0x15, # CHIRPS_PER_ELEV
|
||||
0x16, # GAIN_SHIFT
|
||||
0x20, # RANGE_MODE
|
||||
0x28, # AGC_ENABLE
|
||||
0x29, # AGC_TARGET
|
||||
0x2A, # AGC_ATTACK
|
||||
0x2B, # AGC_DECAY
|
||||
0x2C, # AGC_HOLDOFF
|
||||
0x30, # SELF_TEST_TRIGGER
|
||||
0x31, # SELF_TEST_STATUS
|
||||
0xFF, # STATUS_REQUEST
|
||||
}
|
||||
|
||||
# Replay-adjustable opcodes (re-run signal processing)
|
||||
_REPLAY_ADJUSTABLE_OPCODES = {
|
||||
0x03, # DETECT_THRESHOLD
|
||||
0x21, # CFAR_GUARD
|
||||
0x22, # CFAR_TRAIN
|
||||
0x23, # CFAR_ALPHA
|
||||
0x24, # CFAR_MODE
|
||||
0x25, # CFAR_ENABLE
|
||||
0x26, # MTI_ENABLE
|
||||
0x27, # DC_NOTCH_WIDTH
|
||||
}
|
||||
|
||||
|
||||
def _saturate(val: int, bits: int) -> int:
|
||||
"""Saturate signed value to fit in 'bits' width."""
|
||||
max_pos = (1 << (bits - 1)) - 1
|
||||
max_neg = -(1 << (bits - 1))
|
||||
return max(max_neg, min(max_pos, int(val)))
|
||||
|
||||
|
||||
def _replay_dc_notch(doppler_i: np.ndarray, doppler_q: np.ndarray,
|
||||
width: int) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Bit-accurate DC notch filter (matches radar_system_top.v inline).
|
||||
|
||||
Dual sub-frame notch: doppler_bin[4:0] = {sub_frame, bin[3:0]}.
|
||||
Each 16-bin sub-frame has its own DC at bin 0, so we zero bins
|
||||
where ``bin_within_sf < width`` or ``bin_within_sf > (15 - width + 1)``.
|
||||
"""
|
||||
out_i = doppler_i.copy()
|
||||
out_q = doppler_q.copy()
|
||||
if width == 0:
|
||||
return out_i, out_q
|
||||
n_doppler = doppler_i.shape[1]
|
||||
for dbin in range(n_doppler):
|
||||
bin_within_sf = dbin & 0xF
|
||||
if bin_within_sf < width or bin_within_sf > (15 - width + 1):
|
||||
out_i[:, dbin] = 0
|
||||
out_q[:, dbin] = 0
|
||||
return out_i, out_q
|
||||
|
||||
|
||||
def _replay_cfar(doppler_i: np.ndarray, doppler_q: np.ndarray,
|
||||
guard: int, train: int, alpha_q44: int,
|
||||
mode: int) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Bit-accurate CA-CFAR detector (matches cfar_ca.v).
|
||||
Returns (detect_flags, magnitudes) both (64, 32).
|
||||
"""
|
||||
ALPHA_FRAC_BITS = 4
|
||||
n_range, n_doppler = doppler_i.shape
|
||||
if train == 0:
|
||||
train = 1
|
||||
|
||||
# Compute magnitudes: |I| + |Q| (17-bit unsigned L1 norm)
|
||||
magnitudes = np.zeros((n_range, n_doppler), dtype=np.int64)
|
||||
for r in range(n_range):
|
||||
for d in range(n_doppler):
|
||||
i_val = int(doppler_i[r, d])
|
||||
q_val = int(doppler_q[r, d])
|
||||
abs_i = (-i_val) & 0xFFFF if i_val < 0 else i_val & 0xFFFF
|
||||
abs_q = (-q_val) & 0xFFFF if q_val < 0 else q_val & 0xFFFF
|
||||
magnitudes[r, d] = abs_i + abs_q
|
||||
|
||||
detect_flags = np.zeros((n_range, n_doppler), dtype=np.bool_)
|
||||
MAX_MAG = (1 << 17) - 1
|
||||
|
||||
mode_names = {0: 'CA', 1: 'GO', 2: 'SO'}
|
||||
mode_str = mode_names.get(mode, 'CA')
|
||||
|
||||
for dbin in range(n_doppler):
|
||||
col = magnitudes[:, dbin]
|
||||
for cut in range(n_range):
|
||||
lead_sum, lead_cnt = 0, 0
|
||||
for t in range(1, train + 1):
|
||||
idx = cut - guard - t
|
||||
if 0 <= idx < n_range:
|
||||
lead_sum += int(col[idx])
|
||||
lead_cnt += 1
|
||||
lag_sum, lag_cnt = 0, 0
|
||||
for t in range(1, train + 1):
|
||||
idx = cut + guard + t
|
||||
if 0 <= idx < n_range:
|
||||
lag_sum += int(col[idx])
|
||||
lag_cnt += 1
|
||||
|
||||
if mode_str == 'CA':
|
||||
noise = lead_sum + lag_sum
|
||||
elif mode_str == 'GO':
|
||||
if lead_cnt > 0 and lag_cnt > 0:
|
||||
noise = lead_sum if lead_sum * lag_cnt > lag_sum * lead_cnt else lag_sum
|
||||
else:
|
||||
noise = lead_sum if lead_cnt > 0 else lag_sum
|
||||
elif mode_str == 'SO':
|
||||
if lead_cnt > 0 and lag_cnt > 0:
|
||||
noise = lead_sum if lead_sum * lag_cnt < lag_sum * lead_cnt else lag_sum
|
||||
else:
|
||||
noise = lead_sum if lead_cnt > 0 else lag_sum
|
||||
else:
|
||||
noise = lead_sum + lag_sum
|
||||
|
||||
thr = min((alpha_q44 * noise) >> ALPHA_FRAC_BITS, MAX_MAG)
|
||||
if int(col[cut]) > thr:
|
||||
detect_flags[cut, dbin] = True
|
||||
|
||||
return detect_flags, magnitudes
|
||||
|
||||
|
||||
class ReplayConnection:
|
||||
"""
|
||||
Loads pre-computed .npy arrays (from golden_reference.py co-sim output)
|
||||
and serves them as USB data packets to the dashboard, exercising the full
|
||||
parsing pipeline with real ADI CN0566 radar data.
|
||||
|
||||
Signal processing parameters (CFAR guard/train/alpha/mode, MTI enable,
|
||||
DC notch width) can be adjusted at runtime via write() — the connection
|
||||
re-runs the bit-accurate processing pipeline and rebuilds packets.
|
||||
|
||||
Required npy directory layout (e.g. tb/cosim/real_data/hex/):
|
||||
decimated_range_i.npy (32, 64) int — pre-Doppler range I
|
||||
decimated_range_q.npy (32, 64) int — pre-Doppler range Q
|
||||
doppler_map_i.npy (64, 32) int — Doppler I (no MTI)
|
||||
doppler_map_q.npy (64, 32) int — Doppler Q (no MTI)
|
||||
fullchain_mti_doppler_i.npy (64, 32) int — Doppler I (with MTI)
|
||||
fullchain_mti_doppler_q.npy (64, 32) int — Doppler Q (with MTI)
|
||||
fullchain_cfar_flags.npy (64, 32) bool — CFAR detections
|
||||
fullchain_cfar_mag.npy (64, 32) int — CFAR |I|+|Q| magnitude
|
||||
"""
|
||||
|
||||
def __init__(self, npy_dir: str, use_mti: bool = True,
|
||||
replay_fps: float = 5.0):
|
||||
self._npy_dir = npy_dir
|
||||
self._use_mti = use_mti
|
||||
self._replay_fps = max(replay_fps, 0.1)
|
||||
self._lock = threading.Lock()
|
||||
self.is_open = False
|
||||
self._packets: bytes = b""
|
||||
self._read_offset = 0
|
||||
self._frame_len = 0
|
||||
# Current signal-processing parameters
|
||||
self._mti_enable: bool = use_mti
|
||||
self._dc_notch_width: int = 2
|
||||
self._cfar_guard: int = 2
|
||||
self._cfar_train: int = 8
|
||||
self._cfar_alpha: int = 0x30
|
||||
self._cfar_mode: int = 0 # 0=CA, 1=GO, 2=SO
|
||||
self._cfar_enable: bool = True
|
||||
self._detect_threshold: int = 10000 # RTL default (host_detect_threshold)
|
||||
# Raw source arrays (loaded once, reprocessed on param change)
|
||||
self._dop_mti_i: np.ndarray | None = None
|
||||
self._dop_mti_q: np.ndarray | None = None
|
||||
self._dop_nomti_i: np.ndarray | None = None
|
||||
self._dop_nomti_q: np.ndarray | None = None
|
||||
self._range_i_vec: np.ndarray | None = None
|
||||
self._range_q_vec: np.ndarray | None = None
|
||||
# Rebuild flag
|
||||
self._needs_rebuild = False
|
||||
|
||||
def open(self, _device_index: int = 0) -> bool:
|
||||
try:
|
||||
self._load_arrays()
|
||||
self._packets = self._build_packets()
|
||||
self._frame_len = len(self._packets)
|
||||
self._read_offset = 0
|
||||
self.is_open = True
|
||||
log.info(f"Replay connection opened: {self._npy_dir} "
|
||||
f"(MTI={'ON' if self._mti_enable else 'OFF'}, "
|
||||
f"{self._frame_len} bytes/frame)")
|
||||
return True
|
||||
except (OSError, ValueError, IndexError, struct.error) as e:
|
||||
log.error(f"Replay open failed: {e}")
|
||||
return False
|
||||
|
||||
def close(self):
|
||||
self.is_open = False
|
||||
|
||||
def read(self, size: int = 4096) -> bytes | None:
|
||||
if not self.is_open:
|
||||
return None
|
||||
# Pace reads to target FPS (spread across ~64 reads per frame)
|
||||
time.sleep((1.0 / self._replay_fps) / (NUM_CELLS / 32))
|
||||
with self._lock:
|
||||
# If params changed, rebuild packets
|
||||
if self._needs_rebuild:
|
||||
self._packets = self._build_packets()
|
||||
self._frame_len = len(self._packets)
|
||||
self._read_offset = 0
|
||||
self._needs_rebuild = False
|
||||
end = self._read_offset + size
|
||||
if end <= self._frame_len:
|
||||
chunk = self._packets[self._read_offset:end]
|
||||
self._read_offset = end
|
||||
else:
|
||||
chunk = self._packets[self._read_offset:]
|
||||
self._read_offset = 0
|
||||
return chunk
|
||||
|
||||
def write(self, data: bytes) -> bool:
|
||||
"""
|
||||
Handle host commands in replay mode.
|
||||
Signal-processing params (CFAR, MTI, DC notch) trigger re-processing.
|
||||
Hardware-only params are silently ignored.
|
||||
"""
|
||||
if len(data) < 4:
|
||||
return True
|
||||
word = struct.unpack(">I", data[:4])[0]
|
||||
opcode = (word >> 24) & 0xFF
|
||||
value = word & 0xFFFF
|
||||
|
||||
if opcode in _REPLAY_ADJUSTABLE_OPCODES:
|
||||
changed = False
|
||||
with self._lock:
|
||||
if opcode == 0x03: # DETECT_THRESHOLD
|
||||
if self._detect_threshold != value:
|
||||
self._detect_threshold = value
|
||||
changed = True
|
||||
elif opcode == 0x21: # CFAR_GUARD
|
||||
if self._cfar_guard != value:
|
||||
self._cfar_guard = value
|
||||
changed = True
|
||||
elif opcode == 0x22: # CFAR_TRAIN
|
||||
if self._cfar_train != value:
|
||||
self._cfar_train = value
|
||||
changed = True
|
||||
elif opcode == 0x23: # CFAR_ALPHA
|
||||
if self._cfar_alpha != value:
|
||||
self._cfar_alpha = value
|
||||
changed = True
|
||||
elif opcode == 0x24: # CFAR_MODE
|
||||
if self._cfar_mode != value:
|
||||
self._cfar_mode = value
|
||||
changed = True
|
||||
elif opcode == 0x25: # CFAR_ENABLE
|
||||
new_en = bool(value)
|
||||
if self._cfar_enable != new_en:
|
||||
self._cfar_enable = new_en
|
||||
changed = True
|
||||
elif opcode == 0x26: # MTI_ENABLE
|
||||
new_en = bool(value)
|
||||
if self._mti_enable != new_en:
|
||||
self._mti_enable = new_en
|
||||
changed = True
|
||||
elif opcode == 0x27 and self._dc_notch_width != value: # DC_NOTCH_WIDTH
|
||||
self._dc_notch_width = value
|
||||
changed = True
|
||||
if changed:
|
||||
self._needs_rebuild = True
|
||||
if changed:
|
||||
log.info(f"Replay param updated: opcode=0x{opcode:02X} "
|
||||
f"value={value} — will re-process")
|
||||
else:
|
||||
log.debug(f"Replay param unchanged: opcode=0x{opcode:02X} "
|
||||
f"value={value}")
|
||||
elif opcode in _HARDWARE_ONLY_OPCODES:
|
||||
log.debug(f"Replay: hardware-only opcode 0x{opcode:02X} "
|
||||
f"(ignored in replay mode)")
|
||||
else:
|
||||
log.debug(f"Replay: unknown opcode 0x{opcode:02X} (ignored)")
|
||||
return True
|
||||
|
||||
def _load_arrays(self):
|
||||
"""Load source npy arrays once."""
|
||||
npy = self._npy_dir
|
||||
# MTI Doppler
|
||||
self._dop_mti_i = np.load(
|
||||
os.path.join(npy, "fullchain_mti_doppler_i.npy")).astype(np.int64)
|
||||
self._dop_mti_q = np.load(
|
||||
os.path.join(npy, "fullchain_mti_doppler_q.npy")).astype(np.int64)
|
||||
# Non-MTI Doppler
|
||||
self._dop_nomti_i = np.load(
|
||||
os.path.join(npy, "doppler_map_i.npy")).astype(np.int64)
|
||||
self._dop_nomti_q = np.load(
|
||||
os.path.join(npy, "doppler_map_q.npy")).astype(np.int64)
|
||||
# Range data
|
||||
try:
|
||||
range_i_all = np.load(
|
||||
os.path.join(npy, "decimated_range_i.npy")).astype(np.int64)
|
||||
range_q_all = np.load(
|
||||
os.path.join(npy, "decimated_range_q.npy")).astype(np.int64)
|
||||
self._range_i_vec = range_i_all[-1, :] # last chirp
|
||||
self._range_q_vec = range_q_all[-1, :]
|
||||
except FileNotFoundError:
|
||||
self._range_i_vec = np.zeros(NUM_RANGE_BINS, dtype=np.int64)
|
||||
self._range_q_vec = np.zeros(NUM_RANGE_BINS, dtype=np.int64)
|
||||
|
||||
def _build_packets(self) -> bytes:
|
||||
"""Build a full frame of USB data packets from current params."""
|
||||
# Select Doppler data based on MTI
|
||||
if self._mti_enable:
|
||||
dop_i = self._dop_mti_i
|
||||
dop_q = self._dop_mti_q
|
||||
else:
|
||||
dop_i = self._dop_nomti_i
|
||||
dop_q = self._dop_nomti_q
|
||||
|
||||
# Apply DC notch
|
||||
dop_i, dop_q = _replay_dc_notch(dop_i, dop_q, self._dc_notch_width)
|
||||
|
||||
# Run CFAR
|
||||
if self._cfar_enable:
|
||||
det, _mag = _replay_cfar(
|
||||
dop_i, dop_q,
|
||||
guard=self._cfar_guard,
|
||||
train=self._cfar_train,
|
||||
alpha_q44=self._cfar_alpha,
|
||||
mode=self._cfar_mode,
|
||||
)
|
||||
else:
|
||||
# Simple threshold fallback matching RTL cfar_ca.v:
|
||||
# detect = (|I| + |Q|) > detect_threshold (L1 norm)
|
||||
mag = np.abs(dop_i) + np.abs(dop_q)
|
||||
det = mag > self._detect_threshold
|
||||
|
||||
det_count = int(det.sum())
|
||||
log.info(f"Replay: rebuilt {NUM_CELLS} packets ("
|
||||
f"MTI={'ON' if self._mti_enable else 'OFF'}, "
|
||||
f"DC_notch={self._dc_notch_width}, "
|
||||
f"CFAR={'ON' if self._cfar_enable else 'OFF'} "
|
||||
f"G={self._cfar_guard} T={self._cfar_train} "
|
||||
f"a=0x{self._cfar_alpha:02X} m={self._cfar_mode}, "
|
||||
f"{det_count} detections)")
|
||||
|
||||
range_i = self._range_i_vec
|
||||
range_q = self._range_q_vec
|
||||
|
||||
return self._build_packets_data(range_i, range_q, dop_i, dop_q, det)
|
||||
|
||||
def _build_packets_data(self, range_i, range_q, dop_i, dop_q, det) -> bytes:
|
||||
"""Build 11-byte data packets for FT2232H interface."""
|
||||
buf = bytearray(NUM_CELLS * DATA_PACKET_SIZE)
|
||||
pos = 0
|
||||
for rbin in range(NUM_RANGE_BINS):
|
||||
ri = int(np.clip(range_i[rbin], -32768, 32767))
|
||||
rq = int(np.clip(range_q[rbin], -32768, 32767))
|
||||
rq_bytes = struct.pack(">h", rq)
|
||||
ri_bytes = struct.pack(">h", ri)
|
||||
for dbin in range(NUM_DOPPLER_BINS):
|
||||
di = int(np.clip(dop_i[rbin, dbin], -32768, 32767))
|
||||
dq = int(np.clip(dop_q[rbin, dbin], -32768, 32767))
|
||||
d = 1 if det[rbin, dbin] else 0
|
||||
|
||||
buf[pos] = HEADER_BYTE
|
||||
pos += 1
|
||||
buf[pos:pos+2] = rq_bytes
|
||||
pos += 2
|
||||
buf[pos:pos+2] = ri_bytes
|
||||
pos += 2
|
||||
buf[pos:pos+2] = struct.pack(">h", di)
|
||||
pos += 2
|
||||
buf[pos:pos+2] = struct.pack(">h", dq)
|
||||
pos += 2
|
||||
buf[pos] = d
|
||||
pos += 1
|
||||
buf[pos] = FOOTER_BYTE
|
||||
pos += 1
|
||||
|
||||
return bytes(buf)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
|
||||
Reference in New Issue
Block a user