Mirrors/NawfalMotii79-PLFM_RADAR
1
0
Fork 0
mirror of https://github.com/NawfalMotii79/PLFM_RADAR.git synced 2026-06-09 15:07:14 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
bf399410744be216e8cf8ed2c9c19305e2d3a424
NawfalMotii79-PLFM_RADAR/9_Firmware/9_3_GUI/radar_protocol.py
T
Jason 9d1eb4b11c fix(radar): RX chain corrections, GUI bin alignment, MCU boot ordering 
FPGA — RX chain
  matched_filter_multi_segment.v: drop the gratuitous /4 scaling on
    DDC sign-extended input (was ddc_i[17:2] + ddc_i[1]); use
    ddc_i[15:0] directly. fft_engine has INTERNAL_W=32 with
    saturating 16-bit output, so full 16-bit input is safe. Restores
    ~12 dB of MF input dynamic range.
  radar_receiver_final.v: remove latency_buffer (count-N-pulses-then-
    prime FIFO that left frame 1 with all-zero ref). Replaced with
    a single-FF alignment register on ref_i/ref_q that matches the
    1-FF stage multi_segment ST_PROCESSING uses on adc_data.
    Verified by tb/tb_rxb_fullchain_latency.v — autocorrelation peak
    at bin 0 with peak/mean ~88x.
  doppler_processor.v / mti_canceller.v / cfar_ca.v /
    range_bin_decimator.v / radar_receiver_final.v / radar_system_top.v
    / usb_data_interface_ft2232h.v: switch port and parameter widths
    from RP_NUM_RANGE_BINS / RP_RANGE_BIN_BITS (always 512 / 9-bit)
    to RP_MAX_OUTPUT_BINS / RP_RANGE_BIN_WIDTH_MAX (auto-scales:
    50T 512 / 9-bit, 200T 4096 / 12-bit). Unblocks 200T 20 km mode
    at the RX module boundary; USB wire-protocol extension still
    pending.
  radar_receiver_final.v: doppler_frame_done_prev reset value 0 -> 1
    to prevent false done pulse on cycle 1 when level signal is
    HIGH at reset.
  matched_filter_processing_chain.v: delete the broken `ifdef
    SIMULATION inline behavioural FFT (482 lines removed). It
    produced wrong-bin peaks and 100-1000x weak magnitudes. Chain
    now uses production fft_engine.v + frequency_matched_filter.v
    in both iverilog and Vivado. Iverilog tests are ~38x slower per
    chain pass but produce correct results. Misleading "OK with
    Xilinx IP" comments at three test sites updated since the FFT
    is in-house, not an IP placeholder.

FPGA — testbenches
  tb/tb_rxb_latency_measure.v (new): measures chain internal pipeline
    depth (~2057 cycles, chirp-agnostic).
  tb/tb_rxb_fullchain_latency.v (new): full-chain autocorrelation
    verification — drives ddc with the same chirp samples the loader
    serves as ref, finds peak position and peak/mean.
  tb/tb_matched_filter_processing_chain.v: wait timeouts bumped
    50000 -> 500000 cycles to accommodate production FFT pipeline.

MCU
  main.cpp checkSystemHealthStatus: latch system_emergency_state on
    the error_count > 10 path so the SAFE-MODE blink loop in main()
    actually engages (was bypassed because predicate was false).
  main.cpp: move FPGA reset BEFORE the if(PowerAmplifier) block so
    adar_tr_x is driven LOW (RX commanded externally) before PA Vdd
    reaches 22 V. Old reset block at the original location removed.
  main.cpp MX_GPIO_Init: add GPIO_PIN_12 (FPGA reset) to the
    explicit WritePin(LOW) list so the safe initial state is no
    longer implicit.
  main.cpp checkSystemHealth: rate-limit ADAR1000
    verifyDeviceCommunication (HAL_Delay 1ms x 4 devices = 4 ms
    blocking SPI burst per main-loop iteration) from every-loop to
    every 2 s. readTemperature stays per-loop so over-temp
    detection latency is unchanged.
  USBHandler.cpp processSettingsData: dispatch threshold bumped
    74 -> 82 (matches parser minimum); buffer drained after parse
    attempt (slide remaining bytes left) so a false END find no
    longer sticks the buffer until 256-byte overflow.

GUI
  radar_protocol.py: NUM_RANGE_BINS 64 -> 512 (matches FPGA
    RP_NUM_RANGE_BINS); NUM_CELLS 2048 -> 16384.
  radar_protocol.py _ingest_sample: honor FPGA frame_start bit for
    resync after a USB drop; capture range_profile[rbin] once per
    range bin at dbin == 0 (FPGA emits the same range_i/range_q for
    all 32 Doppler cells of a given range bin; previous accumulator
    inflated the profile 32x).
  v7/models.py RadarSettings: range_resolution 24 -> 6 m (matches
    c/(2*100MHz)*4); max_distance and coverage_radius 1536 -> 3072 m;
    map_size 2000 -> 4000.
  v7/models.py WaveformConfig: n_range_bins 64 -> 512, fft_size
    1024 -> 2048, decimation_factor 16 -> 4.
  GUI_V65_Tk.py: _RANGE_PER_BIN math and stale "~24 m / ~1536 m"
    comments updated.
  test_v7.py: assertion values updated to match new defaults.

Tests
  test_ddc_cosim_fuzz.py: remove unused os/tempfile imports, wrap
    three long lines for ruff E501 compliance.
2026-04-23 05:56:52 +05:45

834 lines
31 KiB
Python
Raw Blame History

#!/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
FT601 USB 3.0 (32-bit, 200T premium board) via ftd3xx
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, ClassVar
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 = 512
NUM_DOPPLER_BINS = 32
NUM_CELLS = NUM_RANGE_BINS * NUM_DOPPLER_BINS # 16384
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])
det_byte = raw[9]
detection = det_byte & 0x01
frame_start = (det_byte >> 7) & 0x01
return {
"range_i": range_i,
"range_q": range_q,
"doppler_i": doppler_i,
"doppler_q": doppler_q,
"detection": detection,
"frame_start": frame_start,
}
@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))
# Bit 7 = frame_start (sample_counter == 0), bit 0 = detection
det_byte = (detection & 0x01) | (0x80 if idx == 0 else 0x00)
pkt.append(det_byte)
pkt.append(FOOTER_BYTE)
buf += pkt
self._mock_seq_idx = (start_idx + num_packets) % NUM_CELLS
return bytes(buf)
# ============================================================================
# FT601 USB 3.0 Connection (premium board only)
# ============================================================================
# Optional ftd3xx import (FTDI's proprietary driver for FT60x USB 3.0 chips).
# pyftdi does NOT support FT601 — it only handles USB 2.0 chips (FT232H, etc.)
try:
import ftd3xx # type: ignore[import-untyped]
FTD3XX_AVAILABLE = True
_Ftd3xxError: type = ftd3xx.FTD3XXError # type: ignore[attr-defined]
except ImportError:
FTD3XX_AVAILABLE = False
_Ftd3xxError = OSError # fallback for type-checking; never raised
class FT601Connection:
"""
FT601 USB 3.0 SuperSpeed FIFO bridge — premium board only.
The FT601 has a 32-bit data bus and runs at 100 MHz.
VID:PID = 0x0403:0x6030 or 0x6031 (FTDI FT60x).
Requires the ``ftd3xx`` library (``pip install ftd3xx`` on Windows,
or ``libft60x`` on Linux). This is FTDI's proprietary USB 3.0 driver;
``pyftdi`` only supports USB 2.0 and will NOT work with FT601.
Public contract matches FT2232HConnection so callers can swap freely.
"""
VID = 0x0403
PID_LIST: ClassVar[list[int]] = [0x6030, 0x6031]
def __init__(self, mock: bool = True):
self._mock = mock
self._dev = None
self._lock = threading.Lock()
self.is_open = False
# Mock state (reuses same synthetic data pattern)
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("FT601 mock device opened (no hardware)")
return True
if not FTD3XX_AVAILABLE:
log.error(
"ftd3xx library required for FT601 hardware — "
"install with: pip install ftd3xx"
)
return False
try:
self._dev = ftd3xx.create(device_index, ftd3xx.OPEN_BY_INDEX)
if self._dev is None:
log.error("No FT601 device found at index %d", device_index)
return False
# Verify chip configuration — only reconfigure if needed.
# setChipConfiguration triggers USB re-enumeration, which
# invalidates the device handle and requires a re-open cycle.
cfg = self._dev.getChipConfiguration()
needs_reconfig = (
cfg.FIFOMode != 0 # 245 FIFO mode
or cfg.ChannelConfig != 0 # 1 channel, 32-bit
or cfg.OptionalFeatureSupport != 0
)
if needs_reconfig:
cfg.FIFOMode = 0
cfg.ChannelConfig = 0
cfg.OptionalFeatureSupport = 0
self._dev.setChipConfiguration(cfg)
# Device re-enumerates — close stale handle, wait, re-open
self._dev.close()
self._dev = None
import time
time.sleep(2.0) # wait for USB re-enumeration
self._dev = ftd3xx.create(device_index, ftd3xx.OPEN_BY_INDEX)
if self._dev is None:
log.error("FT601 not found after reconfiguration")
return False
log.info("FT601 reconfigured and re-opened (index %d)", device_index)
self.is_open = True
log.info("FT601 device opened (index %d)", device_index)
return True
except (OSError, _Ftd3xxError) as e:
log.error("FT601 open failed: %s", e)
self._dev = None
return False
def close(self):
if self._dev is not None:
with contextlib.suppress(Exception):
self._dev.close()
self._dev = None
self.is_open = False
def read(self, size: int = 4096) -> bytes | None:
"""Read raw bytes from FT601. 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._dev.readPipe(0x82, size, raw=True)
return bytes(data) if data else None
except (OSError, _Ftd3xxError) as e:
log.error("FT601 read error: %s", e)
return None
def write(self, data: bytes) -> bool:
"""Write raw bytes to FT601. Data must be 4-byte aligned for 32-bit bus."""
if not self.is_open:
return False
if self._mock:
log.info(f"FT601 mock write: {data.hex()}")
return True
# Pad to 4-byte alignment (FT601 32-bit bus requirement).
# NOTE: Radar commands are already 4 bytes, so this should be a no-op.
remainder = len(data) % 4
if remainder:
data = data + b"\x00" * (4 - remainder)
with self._lock:
try:
written = self._dev.writePipe(0x02, data, raw=True)
return written == len(data)
except (OSError, _Ftd3xxError) as e:
log.error("FT601 write error: %s", e)
return False
def _mock_read(self, size: int) -> bytes:
"""Generate synthetic radar packets (same pattern as FT2232H mock)."""
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
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))
# Bit 7 = frame_start (sample_counter == 0), bit 0 = detection
det_byte = (detection & 0x01) | (0x80 if idx == 0 else 0x00)
pkt.append(det_byte)
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."""
# [GUI-C2 FIX] Use FPGA frame_start bit as the authoritative sync token.
# If FPGA flags frame_start mid-stream (after a USB drop or any glitch),
# finalize whatever we have and re-align to bin (0, 0). Without this the
# count-only sync stays permanently misaligned after a single dropped byte.
if sample.get("frame_start", 0) and self._sample_idx > 0:
self._finalize_frame() # resets _sample_idx to 0 and starts a new frame
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
# [GUI-C4 FIX] FPGA emits the same range_i/range_q for all 32 Doppler
# bins of a given range bin (it's the matched-filter range output,
# repeated per Doppler cell). Accumulating across all 32 inflates
# the profile 32x. Capture once per range bin at the first Doppler
# cell instead.
if dbin == 0:
ri = int(sample.get("range_i", 0))
rq = int(sample.get("range_q", 0))
self._frame.range_profile[rbin] = abs(ri) + abs(rq)
self._sample_idx += 1
if self._sample_idx >= NUM_CELLS:
self._finalize_frame()
def _finalize_frame(self):
"""Complete frame: push to queue, record."""
self._frame.timestamp = time.time()
self._frame.frame_number = self._frame_num
# range_profile is already accumulated from FPGA range_i/range_q
# data in _ingest_sample(). No need to synthesize from doppler magnitude.
# 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
Reference in New Issue View Git Blame Copy Permalink