Jason
24b8442e40
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.
630 lines
23 KiB
Python
630 lines
23 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
AERIS-10 Radar Protocol Layer
|
|
===============================
|
|
Pure-logic module for USB packet parsing and command building.
|
|
No GUI dependencies — safe to import from tests and headless scripts.
|
|
|
|
USB Interface: FT2232H USB 2.0 (8-bit, 50T production board) via pyftdi
|
|
|
|
USB Packet Protocol (11-byte):
|
|
TX (FPGA→Host):
|
|
Data packet: [0xAA] [range_q 2B] [range_i 2B] [dop_re 2B] [dop_im 2B] [det 1B] [0x55]
|
|
Status packet: [0xBB] [status 6x32b] [0x55]
|
|
RX (Host→FPGA):
|
|
Command: 4 bytes received sequentially {opcode, addr, value_hi, value_lo}
|
|
"""
|
|
|
|
import struct
|
|
import time
|
|
import threading
|
|
import queue
|
|
import logging
|
|
import contextlib
|
|
from dataclasses import dataclass, field
|
|
from typing import Any
|
|
from enum import IntEnum
|
|
|
|
|
|
import numpy as np
|
|
|
|
log = logging.getLogger("radar_protocol")
|
|
|
|
# ============================================================================
|
|
# Constants matching usb_data_interface.v
|
|
# ============================================================================
|
|
|
|
HEADER_BYTE = 0xAA
|
|
FOOTER_BYTE = 0x55
|
|
STATUS_HEADER_BYTE = 0xBB
|
|
|
|
# Packet sizes
|
|
DATA_PACKET_SIZE = 11 # 1 + 4 + 2 + 2 + 1 + 1
|
|
STATUS_PACKET_SIZE = 26 # 1 + 24 + 1
|
|
|
|
NUM_RANGE_BINS = 64
|
|
NUM_DOPPLER_BINS = 32
|
|
NUM_CELLS = NUM_RANGE_BINS * NUM_DOPPLER_BINS # 2048
|
|
|
|
WATERFALL_DEPTH = 64
|
|
|
|
|
|
class Opcode(IntEnum):
|
|
"""Host register opcodes — must match radar_system_top.v case(usb_cmd_opcode).
|
|
|
|
FPGA truth table (from radar_system_top.v lines 902-944):
|
|
0x01 host_radar_mode 0x14 host_short_listen_cycles
|
|
0x02 host_trigger_pulse 0x15 host_chirps_per_elev
|
|
0x03 host_detect_threshold 0x16 host_gain_shift
|
|
0x04 host_stream_control 0x20 host_range_mode
|
|
0x10 host_long_chirp_cycles 0x21-0x27 CFAR / MTI / DC-notch
|
|
0x11 host_long_listen_cycles 0x28-0x2C AGC control
|
|
0x12 host_guard_cycles 0x30 host_self_test_trigger
|
|
0x13 host_short_chirp_cycles 0x31/0xFF host_status_request
|
|
"""
|
|
# --- Basic control (0x01-0x04) ---
|
|
RADAR_MODE = 0x01 # 2-bit mode select
|
|
TRIGGER_PULSE = 0x02 # self-clearing one-shot trigger
|
|
DETECT_THRESHOLD = 0x03 # 16-bit detection threshold value
|
|
STREAM_CONTROL = 0x04 # 3-bit stream enable mask
|
|
|
|
# --- Digital gain (0x16) ---
|
|
GAIN_SHIFT = 0x16 # 4-bit digital gain shift
|
|
|
|
# --- Chirp timing (0x10-0x15) ---
|
|
LONG_CHIRP = 0x10
|
|
LONG_LISTEN = 0x11
|
|
GUARD = 0x12
|
|
SHORT_CHIRP = 0x13
|
|
SHORT_LISTEN = 0x14
|
|
CHIRPS_PER_ELEV = 0x15
|
|
|
|
# --- Signal processing (0x20-0x27) ---
|
|
RANGE_MODE = 0x20
|
|
CFAR_GUARD = 0x21
|
|
CFAR_TRAIN = 0x22
|
|
CFAR_ALPHA = 0x23
|
|
CFAR_MODE = 0x24
|
|
CFAR_ENABLE = 0x25
|
|
MTI_ENABLE = 0x26
|
|
DC_NOTCH_WIDTH = 0x27
|
|
|
|
# --- AGC (0x28-0x2C) ---
|
|
AGC_ENABLE = 0x28
|
|
AGC_TARGET = 0x29
|
|
AGC_ATTACK = 0x2A
|
|
AGC_DECAY = 0x2B
|
|
AGC_HOLDOFF = 0x2C
|
|
|
|
# --- Board self-test / status (0x30-0x31, 0xFF) ---
|
|
SELF_TEST_TRIGGER = 0x30
|
|
SELF_TEST_STATUS = 0x31
|
|
STATUS_REQUEST = 0xFF
|
|
|
|
|
|
# ============================================================================
|
|
# Data Structures
|
|
# ============================================================================
|
|
|
|
@dataclass
|
|
class RadarFrame:
|
|
"""One complete radar frame (64 range x 32 Doppler)."""
|
|
timestamp: float = 0.0
|
|
range_doppler_i: np.ndarray = field(
|
|
default_factory=lambda: np.zeros((NUM_RANGE_BINS, NUM_DOPPLER_BINS), dtype=np.int16))
|
|
range_doppler_q: np.ndarray = field(
|
|
default_factory=lambda: np.zeros((NUM_RANGE_BINS, NUM_DOPPLER_BINS), dtype=np.int16))
|
|
magnitude: np.ndarray = field(
|
|
default_factory=lambda: np.zeros((NUM_RANGE_BINS, NUM_DOPPLER_BINS), dtype=np.float64))
|
|
detections: np.ndarray = field(
|
|
default_factory=lambda: np.zeros((NUM_RANGE_BINS, NUM_DOPPLER_BINS), dtype=np.uint8))
|
|
range_profile: np.ndarray = field(
|
|
default_factory=lambda: np.zeros(NUM_RANGE_BINS, dtype=np.float64))
|
|
detection_count: int = 0
|
|
frame_number: int = 0
|
|
|
|
|
|
@dataclass
|
|
class StatusResponse:
|
|
"""Parsed status response from FPGA (6-word / 26-byte packet)."""
|
|
radar_mode: int = 0
|
|
stream_ctrl: int = 0
|
|
cfar_threshold: int = 0
|
|
long_chirp: int = 0
|
|
long_listen: int = 0
|
|
guard: int = 0
|
|
short_chirp: int = 0
|
|
short_listen: int = 0
|
|
chirps_per_elev: int = 0
|
|
range_mode: int = 0
|
|
# Self-test results (word 5, added in Build 26)
|
|
self_test_flags: int = 0 # 5-bit result flags [4:0]
|
|
self_test_detail: int = 0 # 8-bit detail code [7:0]
|
|
self_test_busy: int = 0 # 1-bit busy flag
|
|
# AGC metrics (word 4, added for hybrid AGC)
|
|
agc_current_gain: int = 0 # 4-bit current gain encoding [3:0]
|
|
agc_peak_magnitude: int = 0 # 8-bit peak magnitude [7:0]
|
|
agc_saturation_count: int = 0 # 8-bit saturation count [7:0]
|
|
agc_enable: int = 0 # 1-bit AGC enable readback
|
|
|
|
|
|
# ============================================================================
|
|
# Protocol: Packet Parsing & Building
|
|
# ============================================================================
|
|
|
|
def _to_signed16(val: int) -> int:
|
|
"""Convert unsigned 16-bit integer to signed (two's complement)."""
|
|
val = val & 0xFFFF
|
|
return val - 0x10000 if val >= 0x8000 else val
|
|
|
|
|
|
class RadarProtocol:
|
|
"""
|
|
Parse FPGA→Host packets and build Host→FPGA command words.
|
|
Matches usb_data_interface.v packet format exactly.
|
|
"""
|
|
|
|
@staticmethod
|
|
def build_command(opcode: int, value: int, addr: int = 0) -> bytes:
|
|
"""
|
|
Build a 32-bit command word: {opcode[31:24], addr[23:16], value[15:0]}.
|
|
Returns 4 bytes, big-endian (MSB first).
|
|
"""
|
|
word = ((opcode & 0xFF) << 24) | ((addr & 0xFF) << 16) | (value & 0xFFFF)
|
|
return struct.pack(">I", word)
|
|
|
|
@staticmethod
|
|
def parse_data_packet(raw: bytes) -> dict[str, Any] | None:
|
|
"""
|
|
Parse an 11-byte data packet from the FT2232H byte stream.
|
|
Returns dict with keys: 'range_i', 'range_q', 'doppler_i', 'doppler_q',
|
|
'detection', or None if invalid.
|
|
|
|
Packet format (11 bytes):
|
|
Byte 0: 0xAA (header)
|
|
Bytes 1-2: range_q[15:0] MSB first
|
|
Bytes 3-4: range_i[15:0] MSB first
|
|
Bytes 5-6: doppler_real[15:0] MSB first
|
|
Bytes 7-8: doppler_imag[15:0] MSB first
|
|
Byte 9: {7'b0, cfar_detection}
|
|
Byte 10: 0x55 (footer)
|
|
"""
|
|
if len(raw) < DATA_PACKET_SIZE:
|
|
return None
|
|
if raw[0] != HEADER_BYTE:
|
|
return None
|
|
if raw[10] != FOOTER_BYTE:
|
|
return None
|
|
|
|
range_q = _to_signed16(struct.unpack_from(">H", raw, 1)[0])
|
|
range_i = _to_signed16(struct.unpack_from(">H", raw, 3)[0])
|
|
doppler_i = _to_signed16(struct.unpack_from(">H", raw, 5)[0])
|
|
doppler_q = _to_signed16(struct.unpack_from(">H", raw, 7)[0])
|
|
detection = raw[9] & 0x01
|
|
|
|
return {
|
|
"range_i": range_i,
|
|
"range_q": range_q,
|
|
"doppler_i": doppler_i,
|
|
"doppler_q": doppler_q,
|
|
"detection": detection,
|
|
}
|
|
|
|
@staticmethod
|
|
def parse_status_packet(raw: bytes) -> StatusResponse | None:
|
|
"""
|
|
Parse a status response packet.
|
|
Format: [0xBB] [6x4B status words] [0x55] = 1 + 24 + 1 = 26 bytes
|
|
"""
|
|
if len(raw) < 26:
|
|
return None
|
|
if raw[0] != STATUS_HEADER_BYTE:
|
|
return None
|
|
|
|
words = []
|
|
for i in range(6):
|
|
w = struct.unpack_from(">I", raw, 1 + i * 4)[0]
|
|
words.append(w)
|
|
|
|
if raw[25] != FOOTER_BYTE:
|
|
return None
|
|
|
|
sr = StatusResponse()
|
|
# Word 0: {0xFF[31:24], mode[23:22], stream[21:19], 3'b000[18:16], threshold[15:0]}
|
|
sr.cfar_threshold = words[0] & 0xFFFF
|
|
sr.stream_ctrl = (words[0] >> 19) & 0x07
|
|
sr.radar_mode = (words[0] >> 22) & 0x03
|
|
# Word 1: {long_chirp[31:16], long_listen[15:0]}
|
|
sr.long_listen = words[1] & 0xFFFF
|
|
sr.long_chirp = (words[1] >> 16) & 0xFFFF
|
|
# Word 2: {guard[31:16], short_chirp[15:0]}
|
|
sr.short_chirp = words[2] & 0xFFFF
|
|
sr.guard = (words[2] >> 16) & 0xFFFF
|
|
# Word 3: {short_listen[31:16], 10'd0, chirps_per_elev[5:0]}
|
|
sr.chirps_per_elev = words[3] & 0x3F
|
|
sr.short_listen = (words[3] >> 16) & 0xFFFF
|
|
# Word 4: {agc_current_gain[31:28], agc_peak_magnitude[27:20],
|
|
# agc_saturation_count[19:12], agc_enable[11], 9'd0, range_mode[1:0]}
|
|
sr.range_mode = words[4] & 0x03
|
|
sr.agc_enable = (words[4] >> 11) & 0x01
|
|
sr.agc_saturation_count = (words[4] >> 12) & 0xFF
|
|
sr.agc_peak_magnitude = (words[4] >> 20) & 0xFF
|
|
sr.agc_current_gain = (words[4] >> 28) & 0x0F
|
|
# Word 5: {7'd0, self_test_busy, 8'd0, self_test_detail[7:0],
|
|
# 3'd0, self_test_flags[4:0]}
|
|
sr.self_test_flags = words[5] & 0x1F
|
|
sr.self_test_detail = (words[5] >> 8) & 0xFF
|
|
sr.self_test_busy = (words[5] >> 24) & 0x01
|
|
return sr
|
|
|
|
@staticmethod
|
|
def find_packet_boundaries(buf: bytes) -> list[tuple[int, int, str]]:
|
|
"""
|
|
Scan buffer for packet start markers (0xAA data, 0xBB status).
|
|
Returns list of (start_idx, expected_end_idx, packet_type).
|
|
"""
|
|
packets = []
|
|
i = 0
|
|
while i < len(buf):
|
|
if buf[i] == HEADER_BYTE:
|
|
end = i + DATA_PACKET_SIZE
|
|
if end <= len(buf) and buf[end - 1] == FOOTER_BYTE:
|
|
packets.append((i, end, "data"))
|
|
i = end
|
|
else:
|
|
if end > len(buf):
|
|
break # partial packet at end — leave for residual
|
|
i += 1 # footer mismatch — skip this false header
|
|
elif buf[i] == STATUS_HEADER_BYTE:
|
|
end = i + STATUS_PACKET_SIZE
|
|
if end <= len(buf) and buf[end - 1] == FOOTER_BYTE:
|
|
packets.append((i, end, "status"))
|
|
i = end
|
|
else:
|
|
if end > len(buf):
|
|
break # partial status packet — leave for residual
|
|
i += 1 # footer mismatch — skip
|
|
else:
|
|
i += 1
|
|
return packets
|
|
|
|
|
|
# ============================================================================
|
|
# FT2232H USB 2.0 Connection (pyftdi, 245 Synchronous FIFO)
|
|
# ============================================================================
|
|
|
|
# Optional pyftdi import
|
|
try:
|
|
from pyftdi.ftdi import Ftdi, FtdiError
|
|
PyFtdi = Ftdi
|
|
PYFTDI_AVAILABLE = True
|
|
except ImportError:
|
|
class FtdiError(Exception):
|
|
"""Fallback FTDI error type when pyftdi is unavailable."""
|
|
|
|
PYFTDI_AVAILABLE = False
|
|
|
|
|
|
class FT2232HConnection:
|
|
"""
|
|
FT2232H USB 2.0 Hi-Speed FIFO bridge communication.
|
|
Uses pyftdi in 245 Synchronous FIFO mode (Channel A).
|
|
VID:PID = 0x0403:0x6010 (FTDI default for FT2232H).
|
|
"""
|
|
|
|
VID = 0x0403
|
|
PID = 0x6010
|
|
|
|
def __init__(self, mock: bool = True):
|
|
self._mock = mock
|
|
self._ftdi = None
|
|
self._lock = threading.Lock()
|
|
self.is_open = False
|
|
# Mock state
|
|
self._mock_frame_num = 0
|
|
self._mock_rng = np.random.RandomState(42)
|
|
|
|
def open(self, device_index: int = 0) -> bool:
|
|
if self._mock:
|
|
self.is_open = True
|
|
log.info("FT2232H mock device opened (no hardware)")
|
|
return True
|
|
|
|
if not PYFTDI_AVAILABLE:
|
|
log.error("pyftdi not installed — cannot open real FT2232H device")
|
|
return False
|
|
|
|
try:
|
|
self._ftdi = PyFtdi()
|
|
url = f"ftdi://0x{self.VID:04x}:0x{self.PID:04x}/{device_index + 1}"
|
|
self._ftdi.open_from_url(url)
|
|
# Configure for 245 Synchronous FIFO mode
|
|
self._ftdi.set_bitmode(0xFF, PyFtdi.BitMode.SYNCFF)
|
|
# Set USB transfer size for throughput
|
|
self._ftdi.read_data_set_chunksize(65536)
|
|
self._ftdi.write_data_set_chunksize(65536)
|
|
# Purge buffers
|
|
self._ftdi.purge_buffers()
|
|
self.is_open = True
|
|
log.info(f"FT2232H device opened: {url}")
|
|
return True
|
|
except FtdiError as e:
|
|
log.error(f"FT2232H open failed: {e}")
|
|
return False
|
|
|
|
def close(self):
|
|
if self._ftdi is not None:
|
|
with contextlib.suppress(Exception):
|
|
self._ftdi.close()
|
|
self._ftdi = None
|
|
self.is_open = False
|
|
|
|
def read(self, size: int = 4096) -> bytes | None:
|
|
"""Read raw bytes from FT2232H. Returns None on error/timeout."""
|
|
if not self.is_open:
|
|
return None
|
|
|
|
if self._mock:
|
|
return self._mock_read(size)
|
|
|
|
with self._lock:
|
|
try:
|
|
data = self._ftdi.read_data(size)
|
|
return bytes(data) if data else None
|
|
except FtdiError as e:
|
|
log.error(f"FT2232H read error: {e}")
|
|
return None
|
|
|
|
def write(self, data: bytes) -> bool:
|
|
"""Write raw bytes to FT2232H (4-byte commands)."""
|
|
if not self.is_open:
|
|
return False
|
|
|
|
if self._mock:
|
|
log.info(f"FT2232H mock write: {data.hex()}")
|
|
return True
|
|
|
|
with self._lock:
|
|
try:
|
|
written = self._ftdi.write_data(data)
|
|
return written == len(data)
|
|
except FtdiError as e:
|
|
log.error(f"FT2232H write error: {e}")
|
|
return False
|
|
|
|
def _mock_read(self, size: int) -> bytes:
|
|
"""
|
|
Generate synthetic 11-byte radar data packets for testing.
|
|
Emits packets in sequential FPGA order (range_bin 0..63, doppler_bin
|
|
0..31 within each range bin) so that RadarAcquisition._ingest_sample()
|
|
places them correctly. A target is injected near range bin 20,
|
|
Doppler bin 8.
|
|
"""
|
|
time.sleep(0.05)
|
|
self._mock_frame_num += 1
|
|
|
|
buf = bytearray()
|
|
num_packets = min(NUM_CELLS, size // DATA_PACKET_SIZE)
|
|
start_idx = getattr(self, '_mock_seq_idx', 0)
|
|
|
|
for n in range(num_packets):
|
|
idx = (start_idx + n) % NUM_CELLS
|
|
rbin = idx // NUM_DOPPLER_BINS
|
|
dbin = idx % NUM_DOPPLER_BINS
|
|
|
|
range_i = int(self._mock_rng.normal(0, 100))
|
|
range_q = int(self._mock_rng.normal(0, 100))
|
|
if abs(rbin - 20) < 3:
|
|
range_i += 5000
|
|
range_q += 3000
|
|
|
|
dop_i = int(self._mock_rng.normal(0, 50))
|
|
dop_q = int(self._mock_rng.normal(0, 50))
|
|
if abs(rbin - 20) < 3 and abs(dbin - 8) < 2:
|
|
dop_i += 8000
|
|
dop_q += 4000
|
|
|
|
detection = 1 if (abs(rbin - 20) < 2 and abs(dbin - 8) < 2) else 0
|
|
|
|
# Build compact 11-byte packet
|
|
pkt = bytearray()
|
|
pkt.append(HEADER_BYTE)
|
|
pkt += struct.pack(">h", np.clip(range_q, -32768, 32767))
|
|
pkt += struct.pack(">h", np.clip(range_i, -32768, 32767))
|
|
pkt += struct.pack(">h", np.clip(dop_i, -32768, 32767))
|
|
pkt += struct.pack(">h", np.clip(dop_q, -32768, 32767))
|
|
pkt.append(detection & 0x01)
|
|
pkt.append(FOOTER_BYTE)
|
|
|
|
buf += pkt
|
|
|
|
self._mock_seq_idx = (start_idx + num_packets) % NUM_CELLS
|
|
return bytes(buf)
|
|
|
|
|
|
|
|
|
|
|
|
# ============================================================================
|
|
# Data Recorder (HDF5)
|
|
# ============================================================================
|
|
|
|
try:
|
|
import h5py
|
|
HDF5_AVAILABLE = True
|
|
except ImportError:
|
|
HDF5_AVAILABLE = False
|
|
|
|
|
|
class DataRecorder:
|
|
"""Record radar frames to HDF5 files for offline analysis."""
|
|
|
|
def __init__(self):
|
|
self._file = None
|
|
self._grp = None
|
|
self._frame_count = 0
|
|
self._recording = False
|
|
|
|
@property
|
|
def recording(self) -> bool:
|
|
return self._recording
|
|
|
|
def start(self, filepath: str):
|
|
if not HDF5_AVAILABLE:
|
|
log.error("h5py not installed — HDF5 recording unavailable")
|
|
return
|
|
try:
|
|
self._file = h5py.File(filepath, "w")
|
|
self._file.attrs["creator"] = "AERIS-10 Radar Dashboard"
|
|
self._file.attrs["start_time"] = time.time()
|
|
self._file.attrs["range_bins"] = NUM_RANGE_BINS
|
|
self._file.attrs["doppler_bins"] = NUM_DOPPLER_BINS
|
|
|
|
self._grp = self._file.create_group("frames")
|
|
self._frame_count = 0
|
|
self._recording = True
|
|
log.info(f"Recording started: {filepath}")
|
|
except (OSError, ValueError) as e:
|
|
log.error(f"Failed to start recording: {e}")
|
|
|
|
def record_frame(self, frame: RadarFrame):
|
|
if not self._recording or self._file is None:
|
|
return
|
|
try:
|
|
fg = self._grp.create_group(f"frame_{self._frame_count:06d}")
|
|
fg.attrs["timestamp"] = frame.timestamp
|
|
fg.attrs["frame_number"] = frame.frame_number
|
|
fg.attrs["detection_count"] = frame.detection_count
|
|
fg.create_dataset("magnitude", data=frame.magnitude, compression="gzip")
|
|
fg.create_dataset("range_doppler_i", data=frame.range_doppler_i, compression="gzip")
|
|
fg.create_dataset("range_doppler_q", data=frame.range_doppler_q, compression="gzip")
|
|
fg.create_dataset("detections", data=frame.detections, compression="gzip")
|
|
fg.create_dataset("range_profile", data=frame.range_profile, compression="gzip")
|
|
self._frame_count += 1
|
|
except (OSError, ValueError, TypeError) as e:
|
|
log.error(f"Recording error: {e}")
|
|
|
|
def stop(self):
|
|
if self._file is not None:
|
|
try:
|
|
self._file.attrs["end_time"] = time.time()
|
|
self._file.attrs["total_frames"] = self._frame_count
|
|
self._file.close()
|
|
except (OSError, ValueError, RuntimeError):
|
|
pass
|
|
self._file = None
|
|
self._recording = False
|
|
log.info(f"Recording stopped ({self._frame_count} frames)")
|
|
|
|
|
|
# ============================================================================
|
|
# Radar Data Acquisition Thread
|
|
# ============================================================================
|
|
|
|
class RadarAcquisition(threading.Thread):
|
|
"""
|
|
Background thread: reads from USB (FT2232H), parses 11-byte packets,
|
|
assembles frames, and pushes complete frames to the display queue.
|
|
"""
|
|
|
|
def __init__(self, connection, frame_queue: queue.Queue,
|
|
recorder: DataRecorder | None = None,
|
|
status_callback=None):
|
|
super().__init__(daemon=True)
|
|
self.conn = connection
|
|
self.frame_queue = frame_queue
|
|
self.recorder = recorder
|
|
self._status_callback = status_callback
|
|
self._stop_event = threading.Event()
|
|
self._frame = RadarFrame()
|
|
self._sample_idx = 0
|
|
self._frame_num = 0
|
|
|
|
def stop(self):
|
|
self._stop_event.set()
|
|
|
|
def run(self):
|
|
log.info("Acquisition thread started")
|
|
residual = b""
|
|
while not self._stop_event.is_set():
|
|
chunk = self.conn.read(4096)
|
|
if chunk is None or len(chunk) == 0:
|
|
time.sleep(0.01)
|
|
continue
|
|
|
|
raw = residual + chunk
|
|
packets = RadarProtocol.find_packet_boundaries(raw)
|
|
|
|
# Keep unparsed tail bytes for next iteration
|
|
if packets:
|
|
last_end = packets[-1][1]
|
|
residual = raw[last_end:]
|
|
else:
|
|
# No packets found — keep entire buffer as residual
|
|
# but cap at 2x max packet size to avoid unbounded growth
|
|
max_residual = 2 * max(DATA_PACKET_SIZE, STATUS_PACKET_SIZE)
|
|
residual = raw[-max_residual:] if len(raw) > max_residual else raw
|
|
for start, end, ptype in packets:
|
|
if ptype == "data":
|
|
parsed = RadarProtocol.parse_data_packet(
|
|
raw[start:end])
|
|
if parsed is not None:
|
|
self._ingest_sample(parsed)
|
|
elif ptype == "status":
|
|
status = RadarProtocol.parse_status_packet(raw[start:end])
|
|
if status is not None:
|
|
log.info(f"Status: mode={status.radar_mode} "
|
|
f"stream={status.stream_ctrl}")
|
|
if status.self_test_busy or status.self_test_flags:
|
|
log.info(f"Self-test: busy={status.self_test_busy} "
|
|
f"flags=0b{status.self_test_flags:05b} "
|
|
f"detail=0x{status.self_test_detail:02X}")
|
|
if self._status_callback is not None:
|
|
try:
|
|
self._status_callback(status)
|
|
except Exception as e: # noqa: BLE001
|
|
log.error(f"Status callback error: {e}")
|
|
|
|
log.info("Acquisition thread stopped")
|
|
|
|
def _ingest_sample(self, sample: dict):
|
|
"""Place sample into current frame and emit when complete."""
|
|
rbin = self._sample_idx // NUM_DOPPLER_BINS
|
|
dbin = self._sample_idx % NUM_DOPPLER_BINS
|
|
|
|
if rbin < NUM_RANGE_BINS and dbin < NUM_DOPPLER_BINS:
|
|
self._frame.range_doppler_i[rbin, dbin] = sample["doppler_i"]
|
|
self._frame.range_doppler_q[rbin, dbin] = sample["doppler_q"]
|
|
mag = abs(int(sample["doppler_i"])) + abs(int(sample["doppler_q"]))
|
|
self._frame.magnitude[rbin, dbin] = mag
|
|
if sample.get("detection", 0):
|
|
self._frame.detections[rbin, dbin] = 1
|
|
self._frame.detection_count += 1
|
|
|
|
self._sample_idx += 1
|
|
|
|
if self._sample_idx >= NUM_CELLS:
|
|
self._finalize_frame()
|
|
|
|
def _finalize_frame(self):
|
|
"""Complete frame: compute range profile, push to queue, record."""
|
|
self._frame.timestamp = time.time()
|
|
self._frame.frame_number = self._frame_num
|
|
# Range profile = sum of magnitude across Doppler bins
|
|
self._frame.range_profile = np.sum(self._frame.magnitude, axis=1)
|
|
|
|
# Push to display queue (drop old if backed up)
|
|
try:
|
|
self.frame_queue.put_nowait(self._frame)
|
|
except queue.Full:
|
|
with contextlib.suppress(queue.Empty):
|
|
self.frame_queue.get_nowait()
|
|
self.frame_queue.put_nowait(self._frame)
|
|
|
|
if self.recorder and self.recorder.recording:
|
|
self.recorder.record_frame(self._frame)
|
|
|
|
self._frame_num += 1
|
|
self._frame = RadarFrame()
|
|
self._sample_idx = 0
|