Jason
2106e24952
- Expand ruff config from E/F to 17 rule sets (B, RUF, SIM, PIE, T20, ARG, ERA, A, BLE, RET, ISC, TCH, UP, C4, PERF) - Fix 907 lint errors across all Python files (GUI, FPGA cosim, schematics scripts, simulations, utilities, tools) - Replace all blind except-Exception with specific exception types - Remove commented-out dead code (ERA001) from cosim/simulation files - Modernize typing: deprecated typing.List/Dict/Tuple to builtins - Fix unused args/loop vars, ambiguous unicode, perf anti-patterns - Delete legacy GUI files V1-V4 - Add V7 test suite, requirements files - All CI jobs pass: ruff (0 errors), py_compile, pytest (92/92), MCU tests (20/20), FPGA regression (25/25)
439 lines
14 KiB
Python
439 lines
14 KiB
Python
"""
|
|
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.
|
|
|
|
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
|
|
the actual FPGA packet format (0xAA data 11-byte, 0xBB status 26-byte).
|
|
"""
|
|
|
|
import math
|
|
import time
|
|
import random
|
|
import queue
|
|
import struct
|
|
import logging
|
|
|
|
import numpy as np
|
|
|
|
from PyQt6.QtCore import QThread, QObject, QTimer, pyqtSignal
|
|
|
|
from .models import RadarTarget, GPSData, RadarSettings
|
|
from .hardware import (
|
|
RadarAcquisition,
|
|
RadarFrame,
|
|
StatusResponse,
|
|
DataRecorder,
|
|
STM32USBInterface,
|
|
)
|
|
from .processing import (
|
|
RadarProcessor,
|
|
USBPacketParser,
|
|
apply_pitch_correction,
|
|
)
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
# =============================================================================
|
|
# Utility: polar → geographic
|
|
# =============================================================================
|
|
|
|
def polar_to_geographic(
|
|
radar_lat: float,
|
|
radar_lon: float,
|
|
range_m: float,
|
|
azimuth_deg: float,
|
|
) -> tuple:
|
|
"""
|
|
Convert polar coordinates (range, azimuth) relative to radar
|
|
to geographic (latitude, longitude).
|
|
|
|
azimuth_deg: 0 = North, clockwise.
|
|
Returns (lat, lon).
|
|
"""
|
|
R = 6_371_000 # Earth radius in meters
|
|
|
|
lat1 = math.radians(radar_lat)
|
|
lon1 = math.radians(radar_lon)
|
|
bearing = math.radians(azimuth_deg)
|
|
|
|
lat2 = math.asin(
|
|
math.sin(lat1) * math.cos(range_m / R)
|
|
+ math.cos(lat1) * math.sin(range_m / R) * math.cos(bearing)
|
|
)
|
|
lon2 = lon1 + math.atan2(
|
|
math.sin(bearing) * math.sin(range_m / R) * math.cos(lat1),
|
|
math.cos(range_m / R) - math.sin(lat1) * math.sin(lat2),
|
|
)
|
|
return (math.degrees(lat2), math.degrees(lon2))
|
|
|
|
|
|
# =============================================================================
|
|
# Radar Data Worker (QThread) — production protocol
|
|
# =============================================================================
|
|
|
|
class RadarDataWorker(QThread):
|
|
"""
|
|
Background worker that reads radar data from FT2232H (or ReplayConnection),
|
|
parses 0xAA/0xBB packets via production RadarAcquisition, runs optional
|
|
host-side DSP, and emits PyQt signals with results.
|
|
|
|
This replaces the old V7 worker which used an incompatible packet format.
|
|
Now uses production radar_protocol.py for all packet parsing and frame
|
|
assembly (11-byte 0xAA data packets → 64x32 RadarFrame).
|
|
|
|
Signals:
|
|
frameReady(RadarFrame) — a complete 64x32 radar frame
|
|
statusReceived(object) — StatusResponse from FPGA
|
|
targetsUpdated(list) — list of RadarTarget after host-side DSP
|
|
errorOccurred(str) — error message
|
|
statsUpdated(dict) — frame/byte counters
|
|
"""
|
|
|
|
frameReady = pyqtSignal(object) # RadarFrame
|
|
statusReceived = pyqtSignal(object) # StatusResponse
|
|
targetsUpdated = pyqtSignal(list) # List[RadarTarget]
|
|
errorOccurred = pyqtSignal(str)
|
|
statsUpdated = pyqtSignal(dict)
|
|
|
|
def __init__(
|
|
self,
|
|
connection, # FT2232HConnection or ReplayConnection
|
|
processor: RadarProcessor | None = None,
|
|
recorder: DataRecorder | None = None,
|
|
gps_data_ref: GPSData | None = None,
|
|
settings: RadarSettings | None = None,
|
|
parent=None,
|
|
):
|
|
super().__init__(parent)
|
|
self._connection = connection
|
|
self._processor = processor
|
|
self._recorder = recorder
|
|
self._gps = gps_data_ref
|
|
self._settings = settings or RadarSettings()
|
|
self._running = False
|
|
|
|
# Frame queue for production RadarAcquisition → this thread
|
|
self._frame_queue: queue.Queue = queue.Queue(maxsize=4)
|
|
|
|
# Production acquisition thread (does the actual parsing)
|
|
self._acquisition: RadarAcquisition | None = None
|
|
|
|
# Counters
|
|
self._frame_count = 0
|
|
self._byte_count = 0
|
|
self._error_count = 0
|
|
|
|
def stop(self):
|
|
self._running = False
|
|
if self._acquisition:
|
|
self._acquisition.stop()
|
|
|
|
def run(self):
|
|
"""
|
|
Start production RadarAcquisition thread, then poll its frame queue
|
|
and emit PyQt signals for each complete frame.
|
|
"""
|
|
self._running = True
|
|
|
|
# Create and start the production acquisition thread
|
|
self._acquisition = RadarAcquisition(
|
|
connection=self._connection,
|
|
frame_queue=self._frame_queue,
|
|
recorder=self._recorder,
|
|
status_callback=self._on_status,
|
|
)
|
|
self._acquisition.start()
|
|
logger.info("RadarDataWorker started (production protocol)")
|
|
|
|
while self._running:
|
|
try:
|
|
# Poll for complete frames from production acquisition
|
|
frame: RadarFrame = self._frame_queue.get(timeout=0.1)
|
|
self._frame_count += 1
|
|
|
|
# Emit raw frame
|
|
self.frameReady.emit(frame)
|
|
|
|
# Run host-side DSP if processor is configured
|
|
if self._processor is not None:
|
|
targets = self._run_host_dsp(frame)
|
|
if targets:
|
|
self.targetsUpdated.emit(targets)
|
|
|
|
# Emit stats
|
|
self.statsUpdated.emit({
|
|
"frames": self._frame_count,
|
|
"detection_count": frame.detection_count,
|
|
"errors": self._error_count,
|
|
})
|
|
|
|
except queue.Empty:
|
|
continue
|
|
except (ValueError, IndexError) as e:
|
|
self._error_count += 1
|
|
self.errorOccurred.emit(str(e))
|
|
logger.error(f"RadarDataWorker error: {e}")
|
|
|
|
# Stop acquisition thread
|
|
if self._acquisition:
|
|
self._acquisition.stop()
|
|
self._acquisition.join(timeout=2.0)
|
|
self._acquisition = None
|
|
|
|
logger.info("RadarDataWorker stopped")
|
|
|
|
def _on_status(self, status: StatusResponse):
|
|
"""Callback from production RadarAcquisition on status packet."""
|
|
self.statusReceived.emit(status)
|
|
|
|
def _run_host_dsp(self, frame: RadarFrame) -> list[RadarTarget]:
|
|
"""
|
|
Run host-side DSP on a complete frame.
|
|
This is where DBSCAN clustering, Kalman tracking, and other
|
|
non-timing-critical processing happens.
|
|
|
|
The FPGA already does: FFT, MTI, CFAR, DC notch.
|
|
Host-side DSP adds: clustering, tracking, geo-coordinate mapping.
|
|
|
|
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
|
|
|
|
# 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)
|
|
|
|
# DBSCAN clustering
|
|
if cfg.clustering_enabled and len(targets) > 0:
|
|
clusters = self._processor.clustering(
|
|
targets, cfg.clustering_eps, cfg.clustering_min_samples)
|
|
# Associate and track
|
|
if cfg.tracking_enabled:
|
|
targets = self._processor.association(targets, clusters)
|
|
self._processor.tracking(targets)
|
|
|
|
return targets
|
|
|
|
|
|
# =============================================================================
|
|
# GPS Data Worker (QThread)
|
|
# =============================================================================
|
|
|
|
class GPSDataWorker(QThread):
|
|
"""
|
|
Background worker that reads GPS frames from the STM32 USB CDC interface
|
|
and emits parsed GPSData objects.
|
|
|
|
Signals:
|
|
gpsReceived(GPSData)
|
|
errorOccurred(str)
|
|
"""
|
|
|
|
gpsReceived = pyqtSignal(object) # GPSData
|
|
errorOccurred = pyqtSignal(str)
|
|
|
|
def __init__(
|
|
self,
|
|
stm32: STM32USBInterface,
|
|
usb_parser: USBPacketParser,
|
|
parent=None,
|
|
):
|
|
super().__init__(parent)
|
|
self._stm32 = stm32
|
|
self._parser = usb_parser
|
|
self._running = False
|
|
self._gps_count = 0
|
|
|
|
@property
|
|
def gps_count(self) -> int:
|
|
return self._gps_count
|
|
|
|
def stop(self):
|
|
self._running = False
|
|
|
|
def run(self):
|
|
self._running = True
|
|
while self._running:
|
|
if not (self._stm32 and self._stm32.is_open):
|
|
self.msleep(100)
|
|
continue
|
|
|
|
try:
|
|
data = self._stm32.read_data(64, timeout=100)
|
|
if data:
|
|
gps = self._parser.parse_gps_data(data)
|
|
if gps:
|
|
self._gps_count += 1
|
|
self.gpsReceived.emit(gps)
|
|
except (ValueError, struct.error) as e:
|
|
self.errorOccurred.emit(str(e))
|
|
logger.error(f"GPSDataWorker error: {e}")
|
|
self.msleep(100)
|
|
|
|
|
|
# =============================================================================
|
|
# Target Simulator (Demo Mode) — QTimer-based
|
|
# =============================================================================
|
|
|
|
class TargetSimulator(QObject):
|
|
"""
|
|
Generates simulated radar targets for demo/testing.
|
|
|
|
Uses a QTimer on the main thread (or whichever thread owns this object).
|
|
Emits ``targetsUpdated`` with a list[RadarTarget] on each tick.
|
|
"""
|
|
|
|
targetsUpdated = pyqtSignal(list)
|
|
|
|
def __init__(self, radar_position: GPSData, parent=None):
|
|
super().__init__(parent)
|
|
self._radar_pos = radar_position
|
|
self._targets: list[RadarTarget] = []
|
|
self._next_id = 1
|
|
self._timer = QTimer(self)
|
|
self._timer.timeout.connect(self._tick)
|
|
self._initialize_targets(8)
|
|
|
|
# ---- public API --------------------------------------------------------
|
|
|
|
def start(self, interval_ms: int = 500):
|
|
self._timer.start(interval_ms)
|
|
|
|
def stop(self):
|
|
self._timer.stop()
|
|
|
|
def set_radar_position(self, gps: GPSData):
|
|
self._radar_pos = gps
|
|
|
|
def add_random_target(self):
|
|
self._add_random_target()
|
|
|
|
# ---- internals ---------------------------------------------------------
|
|
|
|
def _initialize_targets(self, count: int):
|
|
for _ in range(count):
|
|
self._add_random_target()
|
|
|
|
def _add_random_target(self):
|
|
range_m = random.uniform(5000, 40000)
|
|
azimuth = random.uniform(0, 360)
|
|
velocity = random.uniform(-100, 100)
|
|
elevation = random.uniform(-5, 45)
|
|
|
|
lat, lon = polar_to_geographic(
|
|
self._radar_pos.latitude,
|
|
self._radar_pos.longitude,
|
|
range_m,
|
|
azimuth,
|
|
)
|
|
|
|
target = RadarTarget(
|
|
id=self._next_id,
|
|
range=range_m,
|
|
velocity=velocity,
|
|
azimuth=azimuth,
|
|
elevation=elevation,
|
|
latitude=lat,
|
|
longitude=lon,
|
|
snr=random.uniform(10, 35),
|
|
timestamp=time.time(),
|
|
track_id=self._next_id,
|
|
classification=random.choice(["aircraft", "drone", "bird", "unknown"]),
|
|
)
|
|
self._next_id += 1
|
|
self._targets.append(target)
|
|
|
|
def _tick(self):
|
|
"""Update all simulated targets and emit."""
|
|
updated: list[RadarTarget] = []
|
|
|
|
for t in self._targets:
|
|
new_range = t.range - t.velocity * 0.5
|
|
if new_range < 500 or new_range > 50000:
|
|
continue # target exits coverage — drop it
|
|
|
|
new_vel = max(-150, min(150, t.velocity + random.uniform(-2, 2)))
|
|
new_az = (t.azimuth + random.uniform(-0.5, 0.5)) % 360
|
|
|
|
lat, lon = polar_to_geographic(
|
|
self._radar_pos.latitude,
|
|
self._radar_pos.longitude,
|
|
new_range,
|
|
new_az,
|
|
)
|
|
|
|
updated.append(RadarTarget(
|
|
id=t.id,
|
|
range=new_range,
|
|
velocity=new_vel,
|
|
azimuth=new_az,
|
|
elevation=t.elevation + random.uniform(-0.1, 0.1),
|
|
latitude=lat,
|
|
longitude=lon,
|
|
snr=t.snr + random.uniform(-1, 1),
|
|
timestamp=time.time(),
|
|
track_id=t.track_id,
|
|
classification=t.classification,
|
|
))
|
|
|
|
# Maintain a reasonable target count
|
|
if len(updated) < 5 or (random.random() < 0.05 and len(updated) < 15):
|
|
self._add_random_target()
|
|
updated.append(self._targets[-1])
|
|
|
|
self._targets = updated
|
|
self.targetsUpdated.emit(updated)
|