Compare commits
31
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
ad2b3e555a | ||
|
|
b88d704126 | ||
|
|
9a9b8ce5c7 | ||
|
|
5102ed751c | ||
|
|
3575ca68f5 | ||
|
|
390015ec39 | ||
|
|
4633a92144 | ||
|
|
cfe81f782b | ||
|
|
a185e61495 | ||
|
|
aef549ea76 | ||
|
|
4947d5a14e | ||
|
|
5bb86b7165 | ||
|
|
552cdc688a | ||
|
|
8f097dfa2e | ||
|
|
515049d2ed | ||
|
|
d283ecfdf1 | ||
|
|
45861f4047 | ||
|
|
61ef8720e1 | ||
|
|
7329750c0c | ||
|
|
60f6c9e567 | ||
|
|
e1dfbec712 | ||
|
|
b8cb572bc4 | ||
|
|
e9a0b43ed3 | ||
|
|
ebf0462d85 | ||
|
|
243963063c | ||
|
|
f0ad4e6185 | ||
|
|
fa9da949df | ||
|
|
e202f57ef8 | ||
|
|
e113ef60ab | ||
|
|
16ccafff69 | ||
|
|
b3339d101c |
+4
-4
@@ -86,7 +86,7 @@ target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
202609144
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
@@ -245,13 +245,13 @@ __marimo__/
|
||||
python_app/runtime
|
||||
SHARE_INTERNET_TO_PI.md
|
||||
|
||||
# Локальная обвязка Claude Code: инструкции, хуки, скилы, конфиги инструментов.
|
||||
# В репозиторий не идёт.
|
||||
# Локальная обвязка Claude Code и Codex: инструкции, хуки, скилы, конфиги инструментов.
|
||||
# В репозиторий не идёт. AGENTS.md — симлинк на CLAUDE.md для Codex.
|
||||
CLAUDE.md
|
||||
AGENTS.md
|
||||
.claude/
|
||||
.graphifyignore
|
||||
pyrightconfig.json
|
||||
/docs/
|
||||
test_end_2/
|
||||
|
||||
# --- device_firmware: PlatformIO / STM32G431 cart remote ---
|
||||
|
||||
Vendored
+3
@@ -0,0 +1,3 @@
|
||||
{
|
||||
"cmake.sourceDirectory": "/home/ayzen/Documents/radar_system/data_acq_and_processing/gpr_cpp"
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,24 +1,39 @@
|
||||
CXX := g++
|
||||
PYTHON ?= .venv/bin/python
|
||||
CXXFLAGS := -std=c++20 -O2 -Wall -Wextra -Wpedantic -pthread -MMD -MP
|
||||
LDFLAGS := -pthread -lrt
|
||||
ORCH_LDFLAGS := $(LDFLAGS) -lusb-1.0
|
||||
VISA_CXXFLAGS ?= -I/usr/include/ni-visa
|
||||
VISA_LDFLAGS := $(LDFLAGS) -lvisa
|
||||
|
||||
BUILD_DIR := build
|
||||
BIN_DIR := $(BUILD_DIR)/bin
|
||||
|
||||
# --- CUDA for the sar3d processor (optional). Its kernels are compiled at run
|
||||
# time by NVRTC and launched through the CUDA driver API, so no nvcc is needed:
|
||||
# only nvrtc.h, cuda.h, libnvrtc and the NVIDIA driver's libcuda. Headers and
|
||||
# libnvrtc come from the pip nvidia-cuda-* packages in .venv (requirements.txt,
|
||||
# x86_64) or from a system toolkit in /usr/local/cuda (Jetson). Without them
|
||||
# data_processor still builds; selecting sar3d shows a CUDA warning.
|
||||
CUDA_ROOTS := $(wildcard $(CURDIR)/.venv/lib/python3*/site-packages/nvidia/*) /usr/local/cuda
|
||||
CUDA_NVRTC_HEADER := $(firstword $(wildcard $(addsuffix /include/nvrtc.h,$(CUDA_ROOTS))))
|
||||
CUDA_DRIVER_HEADER := $(firstword $(wildcard $(addsuffix /include/cuda.h,$(CUDA_ROOTS))))
|
||||
CUDA_NVRTC_LIB := $(firstword $(wildcard $(addsuffix /lib/libnvrtc.so.*,$(CUDA_ROOTS)) $(addsuffix /lib64/libnvrtc.so.*,$(CUDA_ROOTS))))
|
||||
CUDA_DRIVER_LIB := $(firstword $(wildcard /usr/lib/*-linux-gnu/libcuda.so.1 /usr/lib/*-linux-gnu/nvidia/libcuda.so.1 /usr/lib64/libcuda.so.1))
|
||||
ifneq ($(and $(CUDA_NVRTC_HEADER),$(CUDA_DRIVER_HEADER),$(CUDA_NVRTC_LIB),$(CUDA_DRIVER_LIB)),)
|
||||
SAR3D_CXXFLAGS := -DSAR3D_WITH_CUDA $(addprefix -I,$(sort $(dir $(CUDA_NVRTC_HEADER) $(CUDA_DRIVER_HEADER))))
|
||||
# The rpath lets data_processor find the pip libnvrtc without LD_LIBRARY_PATH.
|
||||
SAR3D_LDFLAGS := $(CUDA_NVRTC_LIB) $(CUDA_DRIVER_LIB) -Wl,-rpath,$(dir $(CUDA_NVRTC_LIB))
|
||||
endif
|
||||
# Rewritten only when the CUDA setup changes, so installing (or removing) CUDA
|
||||
# rebuilds the sar3d GPU code instead of keeping a stale build.
|
||||
SAR3D_CUDA_STAMP := $(BUILD_DIR)/sar3d_cuda.flags
|
||||
SAR3D_HOST_FLAGS := -ffp-contract=off
|
||||
$(shell mkdir -p $(BUILD_DIR) && echo '$(SAR3D_CXXFLAGS) $(SAR3D_LDFLAGS)' | cmp -s - $(SAR3D_CUDA_STAMP) || echo '$(SAR3D_CXXFLAGS) $(SAR3D_LDFLAGS)' > $(SAR3D_CUDA_STAMP))
|
||||
|
||||
INCLUDES := \
|
||||
-Idata_acq_and_processing/common_cpp/ipc/include \
|
||||
-Idata_acq_and_processing/common_cpp/config/include \
|
||||
-Idata_acq_and_processing/third_party \
|
||||
-Idata_acq_and_processing/third_party/eigen \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/include \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/interfaces \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/radar \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/switches \
|
||||
-Idata_acq_and_processing/preprocessing/calibration_master/include \
|
||||
-Idata_acq_and_processing/preprocessing/reference_master/include \
|
||||
-Idata_acq_and_processing/preprocessing/bundles/include \
|
||||
-Idata_acq_and_processing/preprocessing/data_preprocessor/include \
|
||||
-Idata_acq_and_processing/processing/processors/include \
|
||||
-Idata_acq_and_processing/processing/data_processor/include \
|
||||
@@ -29,21 +44,8 @@ COMMON_SOURCES := \
|
||||
data_acq_and_processing/common_cpp/ipc/src/shared_types.cpp \
|
||||
data_acq_and_processing/common_cpp/config/src/run_config.cpp
|
||||
|
||||
ORCH_SOURCES := \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_transport.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_protocol.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/radar/remote_compact_m_k209/remote_compact_m_k209_driver.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/switches/h7992_minimal_driver.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/switches/hmc349a_minimal_driver.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/src/main.cpp
|
||||
|
||||
PREPROC_SOURCES := \
|
||||
data_acq_and_processing/preprocessing/calibration_master/src/calibration_master.cpp \
|
||||
data_acq_and_processing/preprocessing/calibration_master/src/channel_bundle_support.cpp \
|
||||
data_acq_and_processing/preprocessing/calibration_master/src/through_calibrator.cpp \
|
||||
data_acq_and_processing/preprocessing/reference_master/src/reference_master.cpp \
|
||||
data_acq_and_processing/preprocessing/bundles/src/channel_bundle_support.cpp \
|
||||
data_acq_and_processing/preprocessing/data_preprocessor/src/data_preprocessor.cpp \
|
||||
data_acq_and_processing/preprocessing/data_preprocessor/src/main.cpp
|
||||
|
||||
@@ -55,88 +57,139 @@ PROCESSOR_SOURCES := \
|
||||
data_acq_and_processing/processing/processors/src/bscan_processor.cpp \
|
||||
data_acq_and_processing/processing/processors/src/gpr_processor.cpp \
|
||||
data_acq_and_processing/processing/processors/src/passthrough_processor.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_processor.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_clean.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_gpu.cpp \
|
||||
data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp \
|
||||
data_acq_and_processing/processing/data_processor/src/data_processor.cpp \
|
||||
data_acq_and_processing/processing/data_processor/src/main.cpp \
|
||||
$(LOCATOR_SOURCES)
|
||||
|
||||
# Kamil ADC collector: standalone L-Card E-502 acquisition binary. It loads the
|
||||
# RadioPhotonic radar (RPH) collector: application-owned L-Card E-502 acquisition binary. It loads the
|
||||
# proprietary libx502api/libe502api at runtime via dlopen, so it builds anywhere
|
||||
# from the vendored headers alone and only needs those libraries present at run
|
||||
# time. Built on demand (not part of `all`) for radar.model == kamil_adc.
|
||||
KAMIL_COLLECTOR_DIR := data_acq_and_processing/kamil_adc_collector
|
||||
# The collector now also drives the RF switches itself (switch-aware mode), so it
|
||||
# reuses the orchestrator's switch drivers and the shared run_config loader, which
|
||||
# pull in the common config/ipc/locator headers and the vendored nlohmann/json.
|
||||
KAMIL_COLLECTOR_INCLUDES := \
|
||||
-I$(KAMIL_COLLECTOR_DIR)/include \
|
||||
-I$(KAMIL_COLLECTOR_DIR)/vendor/lcard \
|
||||
# time. Built on demand (not part of `all`) for radar.model == rph_radar.
|
||||
RPH_COLLECTOR_DIR := data_acq_and_processing/rph_radar_collector
|
||||
# The collector drives H7992 switches from the validated JSON supplied by Python.
|
||||
RPH_COLLECTOR_INCLUDES := \
|
||||
-I$(RPH_COLLECTOR_DIR)/include \
|
||||
-I$(RPH_COLLECTOR_DIR)/vendor/lcard \
|
||||
-Idata_acq_and_processing/common_cpp/ipc/include \
|
||||
-Idata_acq_and_processing/common_cpp/config/include \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/interfaces \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/switches \
|
||||
-Idata_acq_and_processing/common_cpp/hardware/include \
|
||||
-Idata_acq_and_processing/processing/locator/include \
|
||||
-Idata_acq_and_processing/third_party
|
||||
KAMIL_COLLECTOR_LDFLAGS := -pthread -ldl -lutil
|
||||
# Collector sources, including the shared drivers/config loader it reuses. These
|
||||
# are compiled into a private build/kamil/ tree (see rule below) so they never
|
||||
# collide with the orchestrator's objects, which use different compile flags.
|
||||
KAMIL_COLLECTOR_SOURCES := \
|
||||
$(KAMIL_COLLECTOR_DIR)/src/main.cpp \
|
||||
$(KAMIL_COLLECTOR_DIR)/src/tty_protocol_writer.cpp \
|
||||
$(KAMIL_COLLECTOR_DIR)/src/capture_file_writer.cpp \
|
||||
data_acq_and_processing/common_cpp/config/src/run_config.cpp \
|
||||
data_acq_and_processing/common_cpp/ipc/src/shared_types.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/switches/h7992_minimal_driver.cpp \
|
||||
data_acq_and_processing/sweep_orchestrator/device_drivers/switches/hmc349a_minimal_driver.cpp
|
||||
RPH_COLLECTOR_LDFLAGS := -pthread -ldl -lutil
|
||||
# Collector sources, including the shared H7992 driver. These
|
||||
# are compiled into a private build/rph/ tree (see rule below) so they never
|
||||
# collide with pipeline objects, which use different include paths.
|
||||
RPH_COLLECTOR_SOURCES := \
|
||||
$(RPH_COLLECTOR_DIR)/src/main.cpp \
|
||||
$(RPH_COLLECTOR_DIR)/src/tty_protocol_writer.cpp \
|
||||
data_acq_and_processing/common_cpp/hardware/src/h7992_minimal_driver.cpp
|
||||
|
||||
SWEEP_ORCH_OBJS := $(addprefix $(BUILD_DIR)/,$(COMMON_SOURCES:.cpp=.o) $(ORCH_SOURCES:.cpp=.o))
|
||||
PREPROCESSOR_OBJS := $(addprefix $(BUILD_DIR)/,$(COMMON_SOURCES:.cpp=.o) $(PREPROC_SOURCES:.cpp=.o))
|
||||
DATA_PROCESSOR_OBJS := $(addprefix $(BUILD_DIR)/,$(COMMON_SOURCES:.cpp=.o) $(PROCESSOR_SOURCES:.cpp=.o))
|
||||
KAMIL_BUILD_DIR := $(BUILD_DIR)/kamil
|
||||
KAMIL_COLLECTOR_OBJS := $(addprefix $(KAMIL_BUILD_DIR)/,$(notdir $(KAMIL_COLLECTOR_SOURCES:.cpp=.o)))
|
||||
DEPFILES := $(sort $(SWEEP_ORCH_OBJS:.o=.d) $(PREPROCESSOR_OBJS:.o=.d) $(DATA_PROCESSOR_OBJS:.o=.d) $(KAMIL_COLLECTOR_OBJS:.o=.d))
|
||||
RPH_BUILD_DIR := $(BUILD_DIR)/rph
|
||||
RPH_COLLECTOR_OBJS := $(addprefix $(RPH_BUILD_DIR)/,$(RPH_COLLECTOR_SOURCES:.cpp=.o))
|
||||
DEPFILES := $(sort $(PREPROCESSOR_OBJS:.o=.d) $(DATA_PROCESSOR_OBJS:.o=.d) $(RPH_COLLECTOR_OBJS:.o=.d))
|
||||
|
||||
NATIVE_TEST_SOURCES := $(COMMON_SOURCES) $(LOCATOR_SOURCES) \
|
||||
data_acq_and_processing/tests/native_regressions.cpp \
|
||||
data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp \
|
||||
$(filter-out %/main.cpp,$(PREPROC_SOURCES)) \
|
||||
$(RPH_COLLECTOR_DIR)/src/tty_protocol_writer.cpp \
|
||||
data_acq_and_processing/common_cpp/hardware/src/h7992_minimal_driver.cpp
|
||||
NATIVE_BUILD_DIR := $(BUILD_DIR)/native_tests
|
||||
NATIVE_TEST_OBJS := $(addprefix $(NATIVE_BUILD_DIR)/,$(NATIVE_TEST_SOURCES:.cpp=.o))
|
||||
NATIVE_CXXFLAGS := $(CXXFLAGS) -O1 -g $(if $(NATIVE_SANITIZERS),-fsanitize=$(NATIVE_SANITIZERS) -fno-omit-frame-pointer)
|
||||
NATIVE_FLAGS_STAMP := $(NATIVE_BUILD_DIR)/compiler.flags
|
||||
$(shell mkdir -p $(NATIVE_BUILD_DIR) && echo '$(NATIVE_CXXFLAGS)' | cmp -s - $(NATIVE_FLAGS_STAMP) || echo '$(NATIVE_CXXFLAGS)' > $(NATIVE_FLAGS_STAMP))
|
||||
DEPFILES += $(NATIVE_TEST_OBJS:.o=.d) $(RPH_BUILD_DIR)/data_acq_and_processing/tests/collector_regressions.d
|
||||
|
||||
TARGETS := \
|
||||
$(BIN_DIR)/sweep_orchestrator \
|
||||
$(BIN_DIR)/data_preprocessor \
|
||||
$(BIN_DIR)/data_processor
|
||||
|
||||
.PHONY: all clean kamil_adc_collector
|
||||
.PHONY: all clean rph_radar_collector check-native check-sar3d check-gpr
|
||||
|
||||
all: $(TARGETS)
|
||||
|
||||
kamil_adc_collector: $(BIN_DIR)/kamil_adc_collector
|
||||
rph_radar_collector: $(BIN_DIR)/rph_radar_collector
|
||||
|
||||
$(BIN_DIR)/sweep_orchestrator: $(SWEEP_ORCH_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(SWEEP_ORCH_OBJS) -o $@ $(ORCH_LDFLAGS)
|
||||
check-native: $(NATIVE_BUILD_DIR)/native_regressions $(BIN_DIR)/collector_regressions $(NATIVE_BUILD_DIR)/rph_sdk/libx502api.so
|
||||
LD_LIBRARY_PATH=$(abspath $(NATIVE_BUILD_DIR)/rph_sdk) $(BIN_DIR)/collector_regressions
|
||||
$(NATIVE_BUILD_DIR)/native_regressions run_config_examples/run_config_simulator.example.json
|
||||
|
||||
BACKPROJECTION_TEST_SOURCES := data_acq_and_processing/tests/backprojection_fixture.cpp \
|
||||
data_acq_and_processing/common_cpp/ipc/src/shared_types.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_processor.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_clean.cpp \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_gpu.cpp \
|
||||
data_acq_and_processing/processing/processors/src/gpr_processor.cpp
|
||||
BACKPROJECTION_TEST_OBJS := $(addprefix $(NATIVE_BUILD_DIR)/,$(BACKPROJECTION_TEST_SOURCES:.cpp=.o))
|
||||
DEPFILES += $(BACKPROJECTION_TEST_OBJS:.o=.d)
|
||||
|
||||
# The sar3d processor on the real CUDA backend against the notebook reference; without a
|
||||
# usable GPU the test checks the warning and the box rules and skips the rest.
|
||||
check-sar3d: $(NATIVE_BUILD_DIR)/backprojection_fixture
|
||||
BACKPROJECTION_FIXTURE="$(abspath $<)" $(PYTHON) data_acq_and_processing/tests/test_backprojection.py Sar3dTest
|
||||
|
||||
check-gpr: $(NATIVE_BUILD_DIR)/backprojection_fixture
|
||||
BACKPROJECTION_FIXTURE="$(abspath $<)" $(PYTHON) data_acq_and_processing/tests/test_backprojection.py GprTest
|
||||
|
||||
$(NATIVE_BUILD_DIR)/backprojection_fixture: $(BACKPROJECTION_TEST_OBJS)
|
||||
$(CXX) $(NATIVE_CXXFLAGS) $^ -o $@ $(LDFLAGS) $(SAR3D_LDFLAGS)
|
||||
|
||||
$(NATIVE_BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_gpu.o: NATIVE_CXXFLAGS += $(SAR3D_CXXFLAGS)
|
||||
$(NATIVE_BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_%.o: NATIVE_CXXFLAGS += $(SAR3D_HOST_FLAGS)
|
||||
$(NATIVE_BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_gpu.o: $(SAR3D_CUDA_STAMP)
|
||||
|
||||
$(NATIVE_BUILD_DIR)/native_regressions: $(NATIVE_TEST_OBJS)
|
||||
$(CXX) $(NATIVE_CXXFLAGS) $^ -o $@ $(LDFLAGS) -lutil
|
||||
|
||||
$(NATIVE_BUILD_DIR)/%.o: %.cpp $(NATIVE_FLAGS_STAMP)
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(NATIVE_CXXFLAGS) $(INCLUDES) -Idata_acq_and_processing/common_cpp/hardware/include -I$(RPH_COLLECTOR_DIR)/include -c $< -o $@
|
||||
|
||||
$(BIN_DIR)/data_preprocessor: $(PREPROCESSOR_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(PREPROCESSOR_OBJS) -o $@ $(LDFLAGS)
|
||||
|
||||
$(BIN_DIR)/data_processor: $(DATA_PROCESSOR_OBJS)
|
||||
$(BIN_DIR)/data_processor: $(DATA_PROCESSOR_OBJS) $(SAR3D_CUDA_STAMP)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(DATA_PROCESSOR_OBJS) -o $@ $(LDFLAGS)
|
||||
$(CXX) $(DATA_PROCESSOR_OBJS) -o $@ $(LDFLAGS) $(SAR3D_LDFLAGS)
|
||||
$(if $(SAR3D_LDFLAGS),,@echo "note: CUDA not found, selecting sar3d will show a warning")
|
||||
|
||||
# The collector compiles into its own build/kamil/ tree with collector-only
|
||||
# includes (no VISA), so the shared driver/config sources it reuses never collide
|
||||
# with the orchestrator's objects of the same name. Basenames are unique, so a
|
||||
# vpath lets one pattern rule find every source.
|
||||
vpath %.cpp $(sort $(dir $(KAMIL_COLLECTOR_SOURCES)))
|
||||
$(BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_gpu.o: CXXFLAGS += $(SAR3D_CXXFLAGS)
|
||||
$(BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_gpu.o: $(SAR3D_CUDA_STAMP)
|
||||
# The sar3d host code breaks the same ties as NumPy (grid corners on the depth gate, range
|
||||
# rings), so it is not contracted into fused multiply-adds where the target has them.
|
||||
$(BUILD_DIR)/data_acq_and_processing/processing/processors/src/sar3d_%.o: CXXFLAGS += $(SAR3D_HOST_FLAGS)
|
||||
|
||||
$(KAMIL_BUILD_DIR)/%.o: %.cpp
|
||||
# Keep source-relative paths so moving a source also replaces its dependencies.
|
||||
$(RPH_BUILD_DIR)/%.o: %.cpp
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXXFLAGS) $(KAMIL_COLLECTOR_INCLUDES) -c $< -o $@
|
||||
$(CXX) $(CXXFLAGS) $(RPH_COLLECTOR_INCLUDES) -c $< -o $@
|
||||
|
||||
$(BIN_DIR)/kamil_adc_collector: $(KAMIL_COLLECTOR_OBJS)
|
||||
$(RPH_BUILD_DIR)/data_acq_and_processing/tests/collector_regressions.o: data_acq_and_processing/tests/collector_regressions.cpp
|
||||
|
||||
$(NATIVE_BUILD_DIR)/rph_sdk/libx502api.so: data_acq_and_processing/tests/rph_sdk_fixture.cpp
|
||||
@mkdir -p $(@D)
|
||||
$(CXX) -std=c++20 -shared -fPIC -I$(RPH_COLLECTOR_DIR)/vendor/lcard $< -o $@
|
||||
ln -sf libx502api.so $(@D)/libe502api.so
|
||||
|
||||
$(BIN_DIR)/collector_regressions: $(RPH_BUILD_DIR)/data_acq_and_processing/tests/collector_regressions.o $(filter-out %/main.o,$(RPH_COLLECTOR_OBJS))
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(KAMIL_COLLECTOR_OBJS) -o $@ $(KAMIL_COLLECTOR_LDFLAGS)
|
||||
$(CXX) $^ -o $@ $(RPH_COLLECTOR_LDFLAGS)
|
||||
|
||||
$(BIN_DIR)/rph_radar_collector: $(RPH_COLLECTOR_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(RPH_COLLECTOR_OBJS) -o $@ $(RPH_COLLECTOR_LDFLAGS)
|
||||
|
||||
$(BUILD_DIR)/%.o: %.cpp
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXXFLAGS) $(VISA_CXXFLAGS) $(INCLUDES) -c $< -o $@
|
||||
$(CXX) $(CXXFLAGS) $(INCLUDES) -c $< -o $@
|
||||
|
||||
-include $(DEPFILES)
|
||||
|
||||
|
||||
@@ -1,124 +1,84 @@
|
||||
# Radar System
|
||||
|
||||
A real-time radar acquisition and imaging system for vector-network-analyzer (VNA)
|
||||
and ADC front-ends. It drives the radar through a frequency sweep, calibrates and
|
||||
preprocesses the measured S-parameters, runs a selectable imaging/detection
|
||||
processor, and presents the live result in a desktop GUI and an in-process web
|
||||
interface. The same application runs interactively on a workstation or unattended
|
||||
as a headless Raspberry Pi appliance.
|
||||
Программа радиолокационной системы. Она управляет радаром, снимает развёртки
|
||||
коэффициента передачи S21 по всем парам антенн, калибрует их, строит изображения сцены
|
||||
и показывает их в окне программы и в браузере. Найденные объекты она отдаёт внешним
|
||||
программам по TCP. Программа работает на рабочей станции с экраном и на Raspberry Pi 5
|
||||
без оператора.
|
||||
|
||||
## Overview
|
||||
Радары, с которыми она работает:
|
||||
|
||||
The system is split into a **C++ real-time pipeline** and a **Python application**
|
||||
that supervises it and handles visualization and control.
|
||||
|
||||
The pipeline runs as three independent processes connected by lock-free POSIX
|
||||
shared-memory ring buffers. Each stage advances on its own, and a slow consumer
|
||||
never blocks a producer — the rings are latest-wins, so a reader that falls behind
|
||||
simply skips to the freshest frame:
|
||||
|
||||
1. **Acquisition** — `sweep_orchestrator` (or a hardware-specific producer)
|
||||
configures the radar, performs the frequency sweep, and publishes raw S11/S21
|
||||
traces.
|
||||
2. **Preprocessing** — `data_preprocessor` applies through/OSL calibration,
|
||||
reference subtraction, and optional frequency-domain notch filtering.
|
||||
3. **Processing** — `data_processor` runs the selected processing mode and emits
|
||||
results (pass-through traces, B-scan images, or GPR images with detected
|
||||
objects). Detected objects are additionally broadcast to subscribers over a TCP
|
||||
locator server.
|
||||
|
||||
The **Python application** (`python_app`) starts and monitors the C++ binaries,
|
||||
reads the result rings, renders the live view with pyqtgraph, and exposes the same
|
||||
controls plus a pixel-identical live plot through an embedded web UI.
|
||||
|
||||
```
|
||||
radar hardware
|
||||
│
|
||||
▼
|
||||
Acquisition ──[raw ring]──▶ Preprocessing ──[preprocessed ring]──▶ Processing ──[result ring]──▶ GUI / Web
|
||||
│
|
||||
└──▶ TCP locator (detected objects)
|
||||
```
|
||||
|
||||
## Supported hardware
|
||||
|
||||
- **librevna** — single LibreVNA.
|
||||
- **librevna_multi** — synchronized multi-device LibreVNA.
|
||||
- **compact_m_k209** — Compact-M K209 / S2VNA (including Raspberry Pi remote mode).
|
||||
- **sn9000** — SNVNA SN9000 over VISA (pure-Python `@py` backend).
|
||||
- **kamil_adc** — external Kamil ADC acquisition process, with optional laser control.
|
||||
- **mock / simulator** — driver-less mode for development without hardware.
|
||||
|
||||
## Processing modes
|
||||
|
||||
- **pass_through** — magnitude and phase of the selected channel for each combo.
|
||||
- **bscan** — depth-versus-sweep B-scan heatmap.
|
||||
- **gpr** — coherent back-projection GPR imaging with object detection.
|
||||
- **legacy_gpr** — ellipse-intersection MIMO localization (point and extended/region modes).
|
||||
|
||||
## Repository layout
|
||||
|
||||
| Path | Contents |
|
||||
| Радар | Что это |
|
||||
| --- | --- |
|
||||
| `data_acq_and_processing/` | C++ pipeline: `sweep_orchestrator/`, `preprocessing/`, `processing/` (including `locator/`), shared `common_cpp/` (config, IPC rings), and vendored `third_party/` (Eigen, nlohmann/json). |
|
||||
| `python_app/` | Python application: `gui/`, `orchestration/` (process supervisor, shared-memory readers, config), `hardware_full/` (device drivers and services), `webui/`, `storage/`, `models/`, `workflows/`, `scripts/` (producers and tools), and `tests/`. |
|
||||
| `run_config_examples/` | Ready-made configuration profiles, one per hardware setup. |
|
||||
| `deploy/` | systemd daemon installer for Raspberry Pi appliances. |
|
||||
| `build/bin/` | Compiled C++ binaries. |
|
||||
| `device_firmware/`, `docs/` | Device firmware and vendor manuals. |
|
||||
| одиночный LibreVNA | один векторный анализатор цепей и ВЧ-ключи на антеннах |
|
||||
| многоприборная схема | три синхронных LibreVNA; с контроллером ключей — матрица 4 × 4 пар за одну развёртку |
|
||||
| радиофотонный радар | СВЧ-сигнал формируют лазеры, данные оцифровывает АЦП L-Card E-502 или плата RFG |
|
||||
| симулятор | работа без железа: один движущийся точечный отражатель |
|
||||
|
||||
## Requirements
|
||||
Режимы обработки: pass-through (S21 по частоте), B-scan (дальность во времени), GPR
|
||||
(карта земли перед радаром с поиском объектов), 3D SAR (трёхмерное изображение на
|
||||
видеокарте NVIDIA).
|
||||
|
||||
- Linux (POSIX shared memory; GPIO on Raspberry Pi). The deployment target is a
|
||||
**Raspberry Pi 5**; hardware PWM gating for the generator sweep is Pi 5
|
||||
specific (GPIO12/13/18/19, `dtoverlay=pwm-2chan`).
|
||||
- A C++20 compiler and `make` — Eigen and nlohmann/json are vendored, so no extra C++ dependencies are needed.
|
||||
- **Python 3.10 or newer**, in a virtual environment.
|
||||
## Быстрый старт без железа
|
||||
|
||||
## Build and run
|
||||
|
||||
From the repository root:
|
||||
|
||||
```bash
|
||||
python3 -m venv .venv
|
||||
.venv/bin/python -m pip install --upgrade pip
|
||||
.venv/bin/python -m pip install -r requirements.txt
|
||||
make # build the C++ binaries into build/bin/
|
||||
```
|
||||
|
||||
Run the desktop GUI:
|
||||
|
||||
```bash
|
||||
.venv/bin/python -m python_app.gui.main
|
||||
# or, with the build step and single-instance handling:
|
||||
```sh
|
||||
./start.sh
|
||||
```
|
||||
|
||||
`start.sh` supports `--headless` (offscreen, auto-start — for unattended Pi use),
|
||||
`--profile PATH`, `--producer-only`, `--skip-build`, and `--clean-shm`; see
|
||||
`./start.sh --help`. The acquisition device (LibreVNA, Kamil ADC, …) is detected
|
||||
automatically from the active config's `radar.model` — there is no per-device flag.
|
||||
`start.sh` создаёт окружение Python, ставит пакеты и собирает C++. Корневой профиль
|
||||
`run_config.json` — симулятор. В окне нажмите **Preprocessing…** →
|
||||
**Create Neutral S21 Sets**, затем **Start** и выберите режим в списке **Mode**.
|
||||
|
||||
The web interface is served in-process at `http://<host>:8080`. It mirrors the live
|
||||
plot and offers Start / Stop / Capture controls and the active processor settings.
|
||||
Установка, запуск с железом и служба для работы без экрана — в главе
|
||||
[Установка и запуск](docs/installation.md).
|
||||
|
||||
## Configuration
|
||||
## Документация
|
||||
|
||||
The active profile is `run_config.json`. It selects the radar model and sweep, the
|
||||
switch matrix and acquisition combos, the preprocessing calibration/reference
|
||||
assets, the GPR geometry, and the shared-memory ring sizes. Profiles for each
|
||||
hardware setup are provided in `run_config_examples/` — copy one to
|
||||
`run_config.json`, or pass it with `--profile`, to use it.
|
||||
Для работы с радаром:
|
||||
|
||||
## Deployment
|
||||
1. [Как работает радар](docs/principles.md) — физика измерения и обработки для тех,
|
||||
кто не работал с векторными анализаторами и радарами.
|
||||
2. [Оборудование](docs/hardware.md) — приборы, подключение, прошивки.
|
||||
3. [Установка и запуск](docs/installation.md) — установка, `start.sh`, служба,
|
||||
обновление с флешки.
|
||||
4. [Типовые сценарии](docs/workflow.md) — порядок действий от подготовки до
|
||||
сохранения данных.
|
||||
5. [Окно программы](docs/desktop.md) — каждая кнопка, поле и сообщение окна.
|
||||
6. [Режимы обработки](docs/processing.md) — что показывает каждый режим и что делает
|
||||
каждая его настройка.
|
||||
7. [Калибровка и предобработка](docs/preprocessing.md) — наборы, съём, калибровка по
|
||||
объекту.
|
||||
8. [Веб-интерфейс, кнопка и локатор](docs/web.md) — управление по сети, GPIO-кнопка,
|
||||
протокол локатора, работа без экрана.
|
||||
9. [Файл конфигурации](docs/configuration.md) — каждый ключ профиля.
|
||||
10. [Данные на диске](docs/data.md) — форматы наборов, снимков, журналов.
|
||||
11. [Неполадки](docs/troubleshooting.md) — признак, причина, что делать.
|
||||
12. [Словарь](docs/glossary.md).
|
||||
|
||||
On a Raspberry Pi, `deploy/install-daemon.sh` installs a systemd service that runs
|
||||
the application headless and restarts it on failure. A single-instance lock ensures
|
||||
the interactive GUI and the daemon never run at the same time, since they share the
|
||||
radar, the shared-memory rings, and the locator port.
|
||||
Для разработки:
|
||||
|
||||
## Tests
|
||||
- [Устройство программы](docs/architecture.md) — процессы, кольца, кадры, состав кода,
|
||||
где что править.
|
||||
- [Разработка](docs/development.md) — сборка, тесты, эталоны, правила.
|
||||
- [Протокол LibreVNA и кадры колец](device_firmware/librevna_protocol.md),
|
||||
[переключение стимула](device_firmware/per_point_switching.md),
|
||||
[контроллер ключей](device_firmware/switch_controller/README.md),
|
||||
[сборка прошивок](device_firmware/multi_libreVNA_firmware/reproduce/README.md),
|
||||
[протокол пульта тележки](device_firmware/cart_firmware/docs/protocol.md),
|
||||
[коллектор E-502](data_acq_and_processing/rph_radar_collector/README.md).
|
||||
|
||||
```bash
|
||||
.venv/bin/python -m unittest discover -s python_app/tests
|
||||
```
|
||||
## Состав репозитория
|
||||
|
||||
| Каталог | Что |
|
||||
| --- | --- |
|
||||
| `python_app/` | окно программы, драйверы приборов, веб-интерфейс, хранение данных, тесты |
|
||||
| `data_acq_and_processing/` | C++: препроцессор, процессор с режимами обработки и локатором, коллектор E-502, тесты |
|
||||
| `run_config_examples/` | профили под каждое железо |
|
||||
| `run_configs/` | профили для загрузки из веб-интерфейса |
|
||||
| `deploy/` | служба systemd и обновление с флешки |
|
||||
| `device_firmware/` | прошивки LibreVNA, контроллера ключей и пульта тележки |
|
||||
| `docs/` | документация |
|
||||
| `start.sh` | запуск: окружение Python, сборка, программа |
|
||||
| `run_config.json` | профиль по умолчанию — симулятор |
|
||||
| `Makefile` | сборка и тесты C++ |
|
||||
| `requirements.txt` | Python-пакеты |
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::runtime {
|
||||
|
||||
inline std::atomic<bool> stop_requested{false};
|
||||
|
||||
inline auto initialize(int argc, char** argv) -> config::RunConfig {
|
||||
const auto stop = [](int) { stop_requested.store(true, std::memory_order_relaxed); };
|
||||
std::signal(SIGINT, stop);
|
||||
std::signal(SIGTERM, stop);
|
||||
std::signal(SIGPIPE, SIG_IGN);
|
||||
std::string path = "run_config.json";
|
||||
for (int index = 1; index + 1 < argc; ++index) {
|
||||
if (std::string(argv[index]) == "--config") { path = argv[++index]; }
|
||||
}
|
||||
return config::load_run_config(path);
|
||||
}
|
||||
|
||||
class FatalPipelineError : public std::runtime_error {
|
||||
public:
|
||||
using std::runtime_error::runtime_error;
|
||||
};
|
||||
|
||||
// Drop isolated malformed collections; make persistent failures conspicuous.
|
||||
class CollectionErrors {
|
||||
public:
|
||||
void progress() { consecutive_ = 0; }
|
||||
void report(const char* worker, const std::exception& error) {
|
||||
++total_;
|
||||
if (++consecutive_ % 500 == 0) {
|
||||
std::cerr << worker << ": " << consecutive_
|
||||
<< " consecutive failures — likely a permanent configuration error: "
|
||||
<< error.what() << '\n';
|
||||
} else if (total_ % 100 == 1) {
|
||||
std::cerr << worker << ": dropped collection after error (count="
|
||||
<< total_ << "): " << error.what() << '\n';
|
||||
}
|
||||
}
|
||||
private:
|
||||
std::uint64_t total_ = 0;
|
||||
std::uint64_t consecutive_ = 0;
|
||||
};
|
||||
|
||||
} // namespace radar::runtime
|
||||
@@ -14,43 +14,15 @@ enum class DriverMode {
|
||||
Native,
|
||||
};
|
||||
|
||||
enum class SwitchDriverKind {
|
||||
H7992,
|
||||
HMC349A,
|
||||
};
|
||||
|
||||
struct RadarSweepSettings {
|
||||
// Frequency sweep bounds and acquisition settings.
|
||||
float start_hz = 1'000'000.0F;
|
||||
float stop_hz = 6'000'000'000.0F;
|
||||
std::uint32_t points = 501;
|
||||
float if_bandwidth_hz = 1'000.0F;
|
||||
float power_dbm = -10.0F;
|
||||
};
|
||||
|
||||
struct RadarConfig {
|
||||
// Radar device identity and runtime mode.
|
||||
std::string model = "librevna";
|
||||
std::string serial{};
|
||||
std::string remote_host = "127.0.0.1";
|
||||
std::uint32_t remote_port = 50209;
|
||||
DriverMode driver_mode = DriverMode::Mock;
|
||||
float mock_signal_hz = 5'000'000.0F;
|
||||
RadarSweepSettings sweep{};
|
||||
};
|
||||
|
||||
struct SwitchConfig {
|
||||
// One RF switch attached to a specific radar port.
|
||||
std::string name{};
|
||||
DriverMode driver_mode = DriverMode::Mock;
|
||||
SwitchDriverKind driver_kind = SwitchDriverKind::H7992;
|
||||
std::uint32_t radar_port = 0;
|
||||
std::uint32_t positions = 4;
|
||||
std::uint32_t default_position = 0;
|
||||
std::string gpio_chip = "/dev/gpiochip0";
|
||||
std::int32_t pin_a = 17;
|
||||
std::int32_t pin_b = 27;
|
||||
bool invert_logic = false;
|
||||
};
|
||||
|
||||
struct RingEndpointConfig {
|
||||
@@ -62,39 +34,25 @@ struct RingEndpointConfig {
|
||||
|
||||
struct RingsConfig {
|
||||
RingEndpointConfig raw{.name = "/radar_raw"};
|
||||
RingEndpointConfig raw_tap{.name = "/radar_raw_tap"};
|
||||
RingEndpointConfig preprocessed{.name = "/radar_preprocessed"};
|
||||
RingEndpointConfig preprocessed_tap{.name = "/radar_preprocessed_tap"};
|
||||
RingEndpointConfig results{.name = "/radar_results"};
|
||||
};
|
||||
|
||||
struct RuntimeConfig {
|
||||
// Runtime pacing and behavior flags for C++ workers.
|
||||
std::uint32_t settling_ms = 0;
|
||||
// Runtime pacing and live processing configuration.
|
||||
std::uint32_t idle_sleep_ms = 2;
|
||||
bool continuous = true;
|
||||
std::string processing_live_config_path = "python_app/runtime/processing_live.json";
|
||||
};
|
||||
|
||||
struct PreprocessAssetConfig {
|
||||
std::string set_name{};
|
||||
std::string bundle_path{};
|
||||
};
|
||||
|
||||
struct S21PreprocessConfig {
|
||||
PreprocessAssetConfig calibration{};
|
||||
PreprocessAssetConfig reference{};
|
||||
};
|
||||
|
||||
struct S11CalibrationPreprocessConfig {
|
||||
PreprocessAssetConfig open{};
|
||||
PreprocessAssetConfig short_standard{};
|
||||
PreprocessAssetConfig load{};
|
||||
};
|
||||
|
||||
struct S11PreprocessConfig {
|
||||
S11CalibrationPreprocessConfig calibration{};
|
||||
PreprocessAssetConfig reference{};
|
||||
PreprocessAssetConfig object_calibration{};
|
||||
};
|
||||
|
||||
struct PreprocessNotchBandConfig {
|
||||
@@ -112,7 +70,6 @@ struct PreprocessNotchConfig {
|
||||
struct PreprocessConfig {
|
||||
// Channel-specific preprocessing assets selected by Python GUI layer.
|
||||
S21PreprocessConfig s21{};
|
||||
S11PreprocessConfig s11{};
|
||||
PreprocessNotchConfig notch{};
|
||||
};
|
||||
|
||||
@@ -134,15 +91,14 @@ struct GprRxGeometry {
|
||||
};
|
||||
|
||||
struct GprConfig {
|
||||
// Stable collection-level GPR processing settings.
|
||||
float relative_permittivity = 1.0F;
|
||||
// Initial GPR settings; the data_processor can override them through live config.
|
||||
std::vector<GprTxGeometry> tx_geometry{};
|
||||
std::vector<GprRxGeometry> rx_geometry{};
|
||||
};
|
||||
|
||||
struct RunConfig {
|
||||
// Single configuration object shared by all C++ processes.
|
||||
RadarConfig radar{};
|
||||
std::string radar_model = "librevna";
|
||||
SwitchConfig input_switch{};
|
||||
SwitchConfig output_switch{};
|
||||
RingsConfig rings{};
|
||||
@@ -154,5 +110,6 @@ struct RunConfig {
|
||||
};
|
||||
|
||||
[[nodiscard]] auto load_run_config(const std::string& path) -> RunConfig;
|
||||
void validate_gpr_config(const GprConfig& config, const RunConfig& run_config);
|
||||
|
||||
} // namespace radar::config
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <thread>
|
||||
|
||||
namespace radar::config {
|
||||
// Returns true when stop interrupts the delay, checking at least every 50 ms.
|
||||
inline auto wait_or_stop(std::uint32_t delay_ms, const std::atomic<bool>& stop) -> bool {
|
||||
std::uint32_t waited = 0;
|
||||
while (waited < delay_ms && !stop.load(std::memory_order_relaxed)) {
|
||||
const auto chunk = std::min(50U, delay_ms - waited);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(chunk));
|
||||
waited += chunk;
|
||||
}
|
||||
return stop.load(std::memory_order_relaxed);
|
||||
}
|
||||
} // namespace radar::config
|
||||
@@ -65,7 +65,18 @@ using Json = nlohmann::json;
|
||||
if (!value.is_number()) {
|
||||
throw std::runtime_error("Expected JSON number at " + context);
|
||||
}
|
||||
return value.get<double>();
|
||||
const auto numeric = value.get<double>();
|
||||
if (!std::isfinite(numeric)) {
|
||||
throw std::runtime_error("Expected finite number at " + context);
|
||||
}
|
||||
return numeric;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto number_to_f32(double value, const std::string& context) -> float {
|
||||
if (!std::isfinite(value) || std::fabs(value) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("Value out of finite float32 range at " + context);
|
||||
}
|
||||
return static_cast<float>(value);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto number_to_u32(double value, const std::string& context) -> std::uint32_t {
|
||||
@@ -74,7 +85,7 @@ using Json = nlohmann::json;
|
||||
}
|
||||
|
||||
const auto rounded = std::round(value);
|
||||
if (std::fabs(value - rounded) > 0.000001) {
|
||||
if (value != rounded) {
|
||||
throw std::runtime_error("Expected integer value at " + context);
|
||||
}
|
||||
return static_cast<std::uint32_t>(rounded);
|
||||
@@ -82,7 +93,7 @@ using Json = nlohmann::json;
|
||||
|
||||
[[nodiscard]] auto number_to_i32(double value, const std::string& context) -> std::int32_t {
|
||||
const auto rounded = std::round(value);
|
||||
if (std::fabs(value - rounded) > 0.000001) {
|
||||
if (value != rounded) {
|
||||
throw std::runtime_error("Expected integer value at " + context);
|
||||
}
|
||||
|
||||
@@ -135,7 +146,7 @@ using Json = nlohmann::json;
|
||||
|
||||
[[nodiscard]] auto optional_f32(const Json& object, const std::string& key, float fallback) -> float {
|
||||
if (const auto* value = optional_field(object, key); value != nullptr) {
|
||||
return static_cast<float>(as_number(*value, key));
|
||||
return number_to_f32(as_number(*value, key), key);
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
@@ -149,7 +160,6 @@ using Json = nlohmann::json;
|
||||
const auto* asset_obj = as_object(required_field(object, key), asset_context);
|
||||
|
||||
PreprocessAssetConfig asset{};
|
||||
asset.set_name = optional_string(*asset_obj, "set_name", "");
|
||||
asset.bundle_path = optional_string(*asset_obj, "bundle_path", "");
|
||||
return asset;
|
||||
}
|
||||
@@ -173,8 +183,8 @@ using Json = nlohmann::json;
|
||||
}
|
||||
|
||||
PreprocessNotchBandConfig band{};
|
||||
band.low_hz = static_cast<float>(as_number((*band_array)[0], context + ".bands_hz[][0]"));
|
||||
band.high_hz = static_cast<float>(as_number((*band_array)[1], context + ".bands_hz[][1]"));
|
||||
band.low_hz = number_to_f32(as_number((*band_array)[0], context + ".bands_hz[][0]"), context);
|
||||
band.high_hz = number_to_f32(as_number((*band_array)[1], context + ".bands_hz[][1]"), context);
|
||||
notch.bands_hz.push_back(band);
|
||||
}
|
||||
}
|
||||
@@ -231,30 +241,7 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error("Unsupported driver_mode value: " + value);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_switch_driver_kind(const std::string& value) -> SwitchDriverKind {
|
||||
if (value == "h7992") {
|
||||
return SwitchDriverKind::H7992;
|
||||
}
|
||||
if (value == "hmc349a") {
|
||||
return SwitchDriverKind::HMC349A;
|
||||
}
|
||||
throw std::runtime_error("Unsupported switch driver value: " + value);
|
||||
}
|
||||
|
||||
void validate_radar_sweep(const RadarSweepSettings& sweep) {
|
||||
if (sweep.points == 0U) {
|
||||
throw std::runtime_error("radar.sweep.points must be > 0");
|
||||
}
|
||||
if (sweep.stop_hz < sweep.start_hz) {
|
||||
throw std::runtime_error("radar.sweep.stop_hz must be >= start_hz");
|
||||
}
|
||||
}
|
||||
|
||||
void validate_switch_config(const SwitchConfig& config) {
|
||||
if (config.radar_port != 1U && config.radar_port != 2U) {
|
||||
throw std::runtime_error("Switch radar_port must be 1 or 2 for " + config.name);
|
||||
}
|
||||
|
||||
if (config.positions == 0U) {
|
||||
throw std::runtime_error("Switch positions must be > 0 for " + config.name);
|
||||
}
|
||||
@@ -270,21 +257,11 @@ void validate_switch_config(const SwitchConfig& config) {
|
||||
throw std::runtime_error("gpio_chip is required for native switch mode: " + config.name);
|
||||
}
|
||||
|
||||
if (config.driver_kind == SwitchDriverKind::H7992) {
|
||||
if (config.positions > 4U) {
|
||||
throw std::runtime_error("Native H7992 switch driver supports positions <= 4 for " + config.name);
|
||||
}
|
||||
if (config.pin_a < 0 || config.pin_b < 0 || config.pin_a == config.pin_b) {
|
||||
throw std::runtime_error("pin_a/pin_b are invalid for native H7992 mode: " + config.name);
|
||||
}
|
||||
return;
|
||||
if (config.positions > 4U) {
|
||||
throw std::runtime_error("Native H7992 switch driver supports positions <= 4 for " + config.name);
|
||||
}
|
||||
|
||||
if (config.positions > 2U) {
|
||||
throw std::runtime_error("Native HMC349A switch driver supports positions <= 2 for " + config.name);
|
||||
}
|
||||
if (config.pin_a < 0) {
|
||||
throw std::runtime_error("pin_a (control) is required for native HMC349A mode: " + config.name);
|
||||
if (config.pin_a < 0 || config.pin_b < 0 || config.pin_a == config.pin_b) {
|
||||
throw std::runtime_error("pin_a/pin_b are invalid for native H7992 mode: " + config.name);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -297,36 +274,9 @@ void validate_combo_key(const ipc::ComboKey& key, const RunConfig& config) {
|
||||
}
|
||||
}
|
||||
|
||||
void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) {
|
||||
if (!(config.relative_permittivity > 0.0F)) {
|
||||
throw std::runtime_error("gpr.relative_permittivity must be > 0");
|
||||
}
|
||||
|
||||
std::unordered_set<std::uint32_t> seen_tx_positions{};
|
||||
for (const auto& entry : config.tx_geometry) {
|
||||
if (entry.output_pos >= run_config.output_switch.positions) {
|
||||
throw std::runtime_error("gpr.tx_geometry output_pos is out of range");
|
||||
}
|
||||
if (!seen_tx_positions.insert(entry.output_pos).second) {
|
||||
throw std::runtime_error("gpr.tx_geometry contains duplicate output_pos");
|
||||
}
|
||||
}
|
||||
|
||||
std::unordered_set<std::uint32_t> seen_rx_positions{};
|
||||
for (const auto& entry : config.rx_geometry) {
|
||||
if (entry.input_pos >= run_config.input_switch.positions) {
|
||||
throw std::runtime_error("gpr.rx_geometry input_pos is out of range");
|
||||
}
|
||||
if (!seen_rx_positions.insert(entry.input_pos).second) {
|
||||
throw std::runtime_error("gpr.rx_geometry contains duplicate input_pos");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_config(const Json& object, const RunConfig& run_config) -> GprConfig {
|
||||
const auto* gpr_obj = as_object(object, "gpr");
|
||||
GprConfig config{};
|
||||
config.relative_permittivity = optional_f32(*gpr_obj, "relative_permittivity", 1.0F);
|
||||
|
||||
if (const auto* tx_value = optional_field(*gpr_obj, "tx_geometry"); tx_value != nullptr) {
|
||||
const auto* tx_array = as_array(*tx_value, "gpr.tx_geometry");
|
||||
@@ -362,22 +312,24 @@ void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) {
|
||||
|
||||
[[nodiscard]] auto parse_switch_config(
|
||||
const Json& object,
|
||||
const std::string& default_name,
|
||||
std::uint32_t default_radar_port
|
||||
const std::string& default_name
|
||||
) -> SwitchConfig {
|
||||
const auto* switch_obj = as_object(object, default_name);
|
||||
|
||||
SwitchConfig config{};
|
||||
config.name = optional_string(*switch_obj, "name", default_name);
|
||||
config.driver_mode = parse_driver_mode(optional_string(*switch_obj, "driver_mode", "mock"));
|
||||
config.driver_kind = parse_switch_driver_kind(optional_string(*switch_obj, "driver", "h7992"));
|
||||
config.radar_port = optional_u32(*switch_obj, "radar_port", default_radar_port);
|
||||
if (optional_string(*switch_obj, "driver", "h7992") != "h7992") {
|
||||
throw std::runtime_error("Only H7992 switches are supported; check GPIO wiring before updating " + default_name);
|
||||
}
|
||||
if (optional_bool(*switch_obj, "invert_logic", false)) {
|
||||
throw std::runtime_error("Switch invert_logic is unsupported for H7992: " + default_name);
|
||||
}
|
||||
config.positions = optional_u32(*switch_obj, "positions", 4);
|
||||
config.default_position = optional_u32(*switch_obj, "default_position", 0);
|
||||
config.gpio_chip = optional_string(*switch_obj, "gpio_chip", "/dev/gpiochip0");
|
||||
config.pin_a = optional_i32(*switch_obj, "pin_a", 17);
|
||||
config.pin_b = optional_i32(*switch_obj, "pin_b", 27);
|
||||
config.invert_logic = optional_bool(*switch_obj, "invert_logic", false);
|
||||
|
||||
validate_switch_config(config);
|
||||
|
||||
@@ -406,6 +358,12 @@ void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) {
|
||||
if (endpoint.slot_size_bytes == 0U) {
|
||||
throw std::runtime_error("Ring slot_size_bytes must be > 0 for " + key);
|
||||
}
|
||||
if (endpoint.slot_size_bytes % 8U != 0U) {
|
||||
throw std::runtime_error("Ring slot_size_bytes must be a multiple of 8 for " + key);
|
||||
}
|
||||
if (endpoint.name.empty() && key == "preprocessed_tap") {
|
||||
return endpoint;
|
||||
}
|
||||
if (endpoint.name.empty() || endpoint.name.front() != '/') {
|
||||
throw std::runtime_error("Ring name must start with '/' for " + key);
|
||||
}
|
||||
@@ -415,6 +373,35 @@ void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) {
|
||||
|
||||
} // namespace
|
||||
|
||||
void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) {
|
||||
std::unordered_set<std::uint32_t> seen_tx_positions{};
|
||||
for (const auto& entry : config.tx_geometry) {
|
||||
if (!std::isfinite(entry.x_m) || !std::isfinite(entry.y_m) || !std::isfinite(entry.z_m)) {
|
||||
throw std::runtime_error("gpr.tx_geometry coordinates must be finite");
|
||||
}
|
||||
if (entry.output_pos >= run_config.output_switch.positions) {
|
||||
throw std::runtime_error("gpr.tx_geometry output_pos is out of range");
|
||||
}
|
||||
if (!seen_tx_positions.insert(entry.output_pos).second) {
|
||||
throw std::runtime_error("gpr.tx_geometry contains duplicate output_pos");
|
||||
}
|
||||
}
|
||||
|
||||
std::unordered_set<std::uint32_t> seen_rx_positions{};
|
||||
for (const auto& entry : config.rx_geometry) {
|
||||
if (!std::isfinite(entry.x_m) || !std::isfinite(entry.y_m) || !std::isfinite(entry.z_m)) {
|
||||
throw std::runtime_error("gpr.rx_geometry coordinates must be finite");
|
||||
}
|
||||
if (entry.input_pos >= run_config.input_switch.positions) {
|
||||
throw std::runtime_error("gpr.rx_geometry input_pos is out of range");
|
||||
}
|
||||
if (!seen_rx_positions.insert(entry.input_pos).second) {
|
||||
throw std::runtime_error("gpr.rx_geometry contains duplicate input_pos");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
auto load_run_config(const std::string& path) -> RunConfig {
|
||||
std::ifstream stream(path);
|
||||
if (!stream.is_open()) {
|
||||
@@ -438,68 +425,18 @@ auto load_run_config(const std::string& path) -> RunConfig {
|
||||
|
||||
{
|
||||
const auto* radar_obj = as_object(required_field(*root_obj, "radar"), "radar");
|
||||
config.radar.model = optional_string(*radar_obj, "model", "librevna");
|
||||
config.radar.serial = optional_string(*radar_obj, "serial", "");
|
||||
config.radar.remote_host = optional_string(*radar_obj, "remote_host", "127.0.0.1");
|
||||
config.radar.remote_port = optional_u32(*radar_obj, "remote_port", 50209);
|
||||
if (config.radar.remote_host.empty()) {
|
||||
throw std::runtime_error("radar.remote_host must not be empty");
|
||||
}
|
||||
if (config.radar.remote_port == 0U || config.radar.remote_port > 65535U) {
|
||||
throw std::runtime_error("radar.remote_port must be in 1..65535");
|
||||
}
|
||||
config.radar.driver_mode = parse_driver_mode(optional_string(*radar_obj, "driver_mode", "mock"));
|
||||
config.radar.mock_signal_hz = optional_f32(*radar_obj, "mock_signal_hz", 5'000'000.0F);
|
||||
|
||||
const auto* sweep_obj = as_object(required_field(*radar_obj, "sweep"), "radar.sweep");
|
||||
config.radar.sweep.start_hz =
|
||||
static_cast<float>(as_number(required_field(*sweep_obj, "start_hz"), "radar.sweep.start_hz"));
|
||||
config.radar.sweep.stop_hz =
|
||||
static_cast<float>(as_number(required_field(*sweep_obj, "stop_hz"), "radar.sweep.stop_hz"));
|
||||
if (const auto* points_value = optional_field(*sweep_obj, "points"); points_value != nullptr) {
|
||||
config.radar.sweep.points =
|
||||
number_to_u32(as_number(*points_value, "radar.sweep.points"), "radar.sweep.points");
|
||||
} else if (config.radar.model != "kamil_adc") {
|
||||
throw std::runtime_error("Missing required config field: points");
|
||||
}
|
||||
config.radar.sweep.if_bandwidth_hz = static_cast<float>(
|
||||
as_number(required_field(*sweep_obj, "if_bandwidth_hz"), "radar.sweep.if_bandwidth_hz")
|
||||
);
|
||||
config.radar.sweep.power_dbm = optional_f32(*sweep_obj, "stimulus_power_dbm", -10.0F);
|
||||
validate_radar_sweep(config.radar.sweep);
|
||||
config.radar_model = optional_string(*radar_obj, "model", "librevna");
|
||||
}
|
||||
|
||||
{
|
||||
const auto* switches_obj = as_object(required_field(*root_obj, "switches"), "switches");
|
||||
|
||||
if (const auto* port1_value = optional_field(*switches_obj, "port1"); port1_value != nullptr) {
|
||||
const auto* port1_obj = as_object(*port1_value, "switches.port1");
|
||||
const auto* port2_obj = as_object(required_field(*switches_obj, "port2"), "switches.port2");
|
||||
config.output_switch = parse_switch_config(*port1_obj, "port1", 1);
|
||||
config.input_switch = parse_switch_config(*port2_obj, "port2", 2);
|
||||
} else {
|
||||
config.input_switch = parse_switch_config(
|
||||
*as_object(required_field(*switches_obj, "input"), "switches.input"),
|
||||
"input",
|
||||
2
|
||||
);
|
||||
config.output_switch = parse_switch_config(
|
||||
*as_object(required_field(*switches_obj, "output"), "switches.output"),
|
||||
"output",
|
||||
1
|
||||
);
|
||||
}
|
||||
|
||||
if (config.input_switch.radar_port == config.output_switch.radar_port) {
|
||||
throw std::runtime_error("Switch radar_port mapping must be unique for two switches");
|
||||
}
|
||||
config.output_switch = parse_switch_config(required_field(*switches_obj, "port1"), "port1");
|
||||
config.input_switch = parse_switch_config(required_field(*switches_obj, "port2"), "port2");
|
||||
}
|
||||
|
||||
{
|
||||
const auto* run_obj = as_object(required_field(*root_obj, "run"), "run");
|
||||
config.runtime.settling_ms = optional_u32(*run_obj, "settling_ms", 0);
|
||||
config.runtime.idle_sleep_ms = optional_u32(*run_obj, "idle_sleep_ms", 2);
|
||||
config.runtime.continuous = optional_bool(*run_obj, "continuous", true);
|
||||
config.runtime.processing_live_config_path = optional_string(
|
||||
*run_obj,
|
||||
"processing_live_config_path",
|
||||
@@ -540,17 +477,10 @@ auto load_run_config(const std::string& path) -> RunConfig {
|
||||
const auto* s21_obj = as_object(required_field(*preprocess_obj, "s21"), "preprocess.s21");
|
||||
config.preprocess.s21.calibration = parse_preprocess_asset(*s21_obj, "calibration", "preprocess.s21");
|
||||
config.preprocess.s21.reference = parse_preprocess_asset(*s21_obj, "reference", "preprocess.s21");
|
||||
|
||||
const auto* s11_obj = as_object(required_field(*preprocess_obj, "s11"), "preprocess.s11");
|
||||
const auto* s11_calibration_obj =
|
||||
as_object(required_field(*s11_obj, "calibration"), "preprocess.s11.calibration");
|
||||
config.preprocess.s11.calibration.open =
|
||||
parse_preprocess_asset(*s11_calibration_obj, "open", "preprocess.s11.calibration");
|
||||
config.preprocess.s11.calibration.short_standard =
|
||||
parse_preprocess_asset(*s11_calibration_obj, "short", "preprocess.s11.calibration");
|
||||
config.preprocess.s11.calibration.load =
|
||||
parse_preprocess_asset(*s11_calibration_obj, "load", "preprocess.s11.calibration");
|
||||
config.preprocess.s11.reference = parse_preprocess_asset(*s11_obj, "reference", "preprocess.s11");
|
||||
if (optional_field(*s21_obj, "object_calibration") != nullptr) {
|
||||
config.preprocess.s21.object_calibration =
|
||||
parse_preprocess_asset(*s21_obj, "object_calibration", "preprocess.s21");
|
||||
}
|
||||
|
||||
if (const auto* notch_value = optional_field(*preprocess_obj, "notch"); notch_value != nullptr) {
|
||||
config.preprocess.notch = parse_preprocess_notch(*as_object(*notch_value, "preprocess.notch"), "preprocess.notch");
|
||||
@@ -566,7 +496,6 @@ auto load_run_config(const std::string& path) -> RunConfig {
|
||||
{
|
||||
const auto* rings_obj = as_object(required_field(*root_obj, "rings"), "rings");
|
||||
config.rings.raw = parse_ring_endpoint(*rings_obj, "raw", config.rings.raw);
|
||||
config.rings.raw_tap = parse_ring_endpoint(*rings_obj, "raw_tap", config.rings.raw_tap);
|
||||
config.rings.preprocessed = parse_ring_endpoint(*rings_obj, "preprocessed", config.rings.preprocessed);
|
||||
config.rings.preprocessed_tap = parse_ring_endpoint(
|
||||
*rings_obj,
|
||||
|
||||
+5
-7
@@ -4,7 +4,6 @@
|
||||
#include <string>
|
||||
|
||||
#include "run_config.hpp"
|
||||
#include "switch_driver.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
@@ -27,15 +26,14 @@ struct H7992MinimalDriverSettings {
|
||||
* Native mode uses Linux GPIO character-device API (uAPI v2).
|
||||
* Mock mode keeps state in memory without touching hardware.
|
||||
*/
|
||||
class H7992MinimalDriver final : public SwitchDriver {
|
||||
class H7992MinimalDriver {
|
||||
public:
|
||||
explicit H7992MinimalDriver(H7992MinimalDriverSettings settings);
|
||||
~H7992MinimalDriver() override; // Leaves RF path safe even if close() was skipped.
|
||||
~H7992MinimalDriver(); // Leaves RF path safe even if close() was skipped.
|
||||
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto position_count() const -> std::uint32_t override;
|
||||
void switch_to(std::uint32_t position) override;
|
||||
void open();
|
||||
void close();
|
||||
void switch_to(std::uint32_t position);
|
||||
|
||||
private:
|
||||
void open_native();
|
||||
-4
@@ -83,10 +83,6 @@ void H7992MinimalDriver::close() {
|
||||
is_open_ = false;
|
||||
}
|
||||
|
||||
auto H7992MinimalDriver::position_count() const -> std::uint32_t {
|
||||
return settings_.positions;
|
||||
}
|
||||
|
||||
void H7992MinimalDriver::switch_to(std::uint32_t position) {
|
||||
if (!is_open_) {
|
||||
throw std::runtime_error("Switch driver is not open for " + settings_.name);
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@@ -26,16 +27,15 @@ struct ComboKey {
|
||||
|
||||
struct ComboKeyHash {
|
||||
[[nodiscard]] auto operator()(const ComboKey& key) const noexcept -> std::size_t {
|
||||
return (static_cast<std::size_t>(key.input_pos) << 32U) ^ static_cast<std::size_t>(key.output_pos);
|
||||
const auto packed = (static_cast<std::uint64_t>(key.input_pos) << 32U) | key.output_pos;
|
||||
return std::hash<std::uint64_t>{}(packed);
|
||||
}
|
||||
};
|
||||
|
||||
struct SweepTraceBlock {
|
||||
ComboKey combo{};
|
||||
// Frequency axis in Hz. Must have the same size as `s11` and `s21`.
|
||||
// Frequency axis in Hz. Must have the same size as `s21`.
|
||||
std::vector<float> frequency_hz{};
|
||||
// Complex S11 samples for matching frequency points.
|
||||
std::vector<Complex32> s11{};
|
||||
// Complex S21 samples for matching frequency points.
|
||||
std::vector<Complex32> s21{};
|
||||
// Monotonic window in which THIS trace's sweep was measured, excluding the
|
||||
@@ -90,11 +90,12 @@ struct ResultBlock {
|
||||
struct ResultCollection {
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t monotonic_ns = 0;
|
||||
// Wall-clock duration of `process_collection()` for this collection,
|
||||
// measured on the data_processor side with a monotonic clock. Used by the
|
||||
// Python pipeline to report processing-time metrics. Zero is a valid
|
||||
// "unmeasured" sentinel for legacy producers.
|
||||
// Processing duration and acquisition identity, independent of stage completion time.
|
||||
std::uint64_t processing_duration_ns = 0;
|
||||
std::uint64_t settings_revision = 0;
|
||||
std::uint64_t capture_start_ns = 0;
|
||||
std::uint64_t capture_end_ns = 0;
|
||||
float speed_m_s = 0.0F;
|
||||
std::vector<ResultPayload> collection_payloads{};
|
||||
std::vector<ResultBlock> blocks{};
|
||||
};
|
||||
@@ -109,7 +110,6 @@ struct ResultCollection {
|
||||
-> PreprocessedCollection;
|
||||
|
||||
[[nodiscard]] auto serialize_result_collection(const ResultCollection& collection) -> std::vector<std::uint8_t>;
|
||||
[[nodiscard]] auto deserialize_result_collection(std::span<const std::uint8_t> bytes) -> ResultCollection;
|
||||
|
||||
// Monotonic timestamp helper used for produced collections.
|
||||
[[nodiscard]] auto current_monotonic_ns() -> std::uint64_t;
|
||||
|
||||
@@ -25,19 +25,11 @@ class ShmRing {
|
||||
std::uint32_t slot_size_bytes
|
||||
) -> ShmRing;
|
||||
|
||||
// Opens ring by name and validates wire-format header fields.
|
||||
[[nodiscard]] static auto open_existing(const std::string& name) -> ShmRing;
|
||||
// Removes shared memory object by name (safe if it does not exist).
|
||||
static void unlink_ring(const std::string& name);
|
||||
|
||||
[[nodiscard]] auto is_open() const -> bool;
|
||||
[[nodiscard]] auto capacity() const -> std::uint32_t;
|
||||
[[nodiscard]] auto slot_size_bytes() const -> std::uint32_t;
|
||||
[[nodiscard]] auto dropped_count() const -> std::uint64_t;
|
||||
|
||||
// Pushes payload with overwrite-oldest policy on overflow.
|
||||
[[nodiscard]] auto push(std::span<const std::uint8_t> payload) -> bool;
|
||||
// Pops the next payload if available.
|
||||
// Pops the next unread payload; false only when none remains.
|
||||
[[nodiscard]] auto pop(std::vector<std::uint8_t>& payload) -> bool;
|
||||
|
||||
private:
|
||||
@@ -53,11 +45,14 @@ class ShmRing {
|
||||
[[nodiscard]] auto slot_stride_bytes() const -> std::size_t;
|
||||
[[nodiscard]] auto slot_header(std::uint64_t sequence) const -> SlotHeader*;
|
||||
[[nodiscard]] auto slot_payload(SlotHeader* slot) const -> std::uint8_t*;
|
||||
void advance_read_seq(std::uint64_t target);
|
||||
|
||||
void close();
|
||||
static void validate_name(const std::string& name);
|
||||
// Validates that mapped_size covers the geometry declared in the header
|
||||
// (fix #50: reject undersized/corrupt mappings to prevent OOB slot access).
|
||||
[[nodiscard]] static auto checked_mapped_size(
|
||||
std::uint32_t capacity, std::uint32_t slot_size_bytes, const std::string& name
|
||||
) -> std::size_t;
|
||||
// Reject undersized/corrupt mappings before accessing any slot.
|
||||
static void validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name);
|
||||
};
|
||||
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#include "shared_types.hpp"
|
||||
|
||||
#include <bit>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
@@ -12,9 +13,11 @@
|
||||
namespace radar::ipc {
|
||||
namespace {
|
||||
|
||||
constexpr std::uint32_t kRawCollectionMagic = 0x32574152U; // RAW2
|
||||
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x32525050U; // PRP2
|
||||
constexpr std::uint32_t kResultCollectionMagic = 0x324C5352U; // RSL2
|
||||
static_assert(std::endian::native == std::endian::little, "IPC requires a little-endian host");
|
||||
|
||||
constexpr std::uint32_t kRawCollectionMagic = 0x33574152U; // RAW3
|
||||
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x33505250U; // PRP3
|
||||
constexpr std::uint32_t kResultCollectionMagic = 0x334C5352U; // RSL3
|
||||
|
||||
template <typename T>
|
||||
concept TriviallySerializable = std::is_trivially_copyable_v<T>;
|
||||
@@ -75,16 +78,6 @@ class BinaryReader {
|
||||
return value;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_string() -> std::string {
|
||||
const auto size = read<std::uint16_t>();
|
||||
ensure_available(size);
|
||||
|
||||
const auto* begin = reinterpret_cast<const char*>(bytes_.data() + offset_);
|
||||
std::string value(begin, begin + size);
|
||||
offset_ += size;
|
||||
return value;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_consumed() const -> bool {
|
||||
return offset_ == bytes_.size();
|
||||
}
|
||||
@@ -95,7 +88,7 @@ class BinaryReader {
|
||||
|
||||
private:
|
||||
void ensure_available(std::size_t size) const {
|
||||
if (offset_ + size > bytes_.size()) {
|
||||
if (size > bytes_.size() - offset_) {
|
||||
throw std::runtime_error("Unexpected end of serialized payload");
|
||||
}
|
||||
}
|
||||
@@ -106,7 +99,6 @@ class BinaryReader {
|
||||
|
||||
void write_trace_block(BinaryWriter& writer, const SweepTraceBlock& trace) {
|
||||
ensure_equal_sizes(trace.frequency_hz.size(), trace.s21.size(), "Sweep trace");
|
||||
ensure_equal_sizes(trace.frequency_hz.size(), trace.s11.size(), "Sweep trace S11");
|
||||
|
||||
writer.write(trace.combo.input_pos);
|
||||
writer.write(trace.combo.output_pos);
|
||||
@@ -116,11 +108,6 @@ void write_trace_block(BinaryWriter& writer, const SweepTraceBlock& trace) {
|
||||
writer.write(frequency_hz);
|
||||
}
|
||||
|
||||
for (const auto& point : trace.s11) {
|
||||
writer.write(point.re);
|
||||
writer.write(point.im);
|
||||
}
|
||||
|
||||
for (const auto& point : trace.s21) {
|
||||
writer.write(point.re);
|
||||
writer.write(point.im);
|
||||
@@ -132,9 +119,8 @@ void write_trace_block(BinaryWriter& writer, const SweepTraceBlock& trace) {
|
||||
// request gigabytes and crash the process (length_error/bad_alloc/OOM-kill). The
|
||||
// payload itself is already bounded by the ring slot size, so any count whose
|
||||
// minimum encoding cannot fit in the bytes still available is structurally invalid.
|
||||
void require_count_fits(std::uint32_t count, std::size_t min_bytes_each, BinaryReader& reader) {
|
||||
if (min_bytes_each != 0U
|
||||
&& static_cast<std::uint64_t>(count) * min_bytes_each > reader.remaining_bytes()) {
|
||||
void require_count_fits(std::uint64_t count, std::size_t min_bytes_each, BinaryReader& reader) {
|
||||
if (min_bytes_each != 0U && count > reader.remaining_bytes() / min_bytes_each) {
|
||||
throw std::runtime_error("Declared element count exceeds remaining payload bytes");
|
||||
}
|
||||
}
|
||||
@@ -145,23 +131,15 @@ void require_count_fits(std::uint32_t count, std::size_t min_bytes_each, BinaryR
|
||||
trace.combo.output_pos = reader.read<std::uint32_t>();
|
||||
|
||||
const auto point_count = reader.read<std::uint32_t>();
|
||||
// Each point encodes frequency (4B) + s11 (8B) + s21 (8B) = 20 bytes.
|
||||
require_count_fits(point_count, 20U, reader);
|
||||
// Each point encodes frequency (4B) + s21 (8B) = 12 bytes.
|
||||
require_count_fits(point_count, 12U, reader);
|
||||
trace.frequency_hz.reserve(point_count);
|
||||
trace.s11.reserve(point_count);
|
||||
trace.s21.reserve(point_count);
|
||||
|
||||
for (std::uint32_t index = 0; index < point_count; ++index) {
|
||||
trace.frequency_hz.push_back(reader.read<float>());
|
||||
}
|
||||
|
||||
for (std::uint32_t index = 0; index < point_count; ++index) {
|
||||
trace.s11.push_back(Complex32{
|
||||
.re = reader.read<float>(),
|
||||
.im = reader.read<float>(),
|
||||
});
|
||||
}
|
||||
|
||||
for (std::uint32_t index = 0; index < point_count; ++index) {
|
||||
trace.s21.push_back(Complex32{
|
||||
.re = reader.read<float>(),
|
||||
@@ -172,10 +150,7 @@ void require_count_fits(std::uint32_t count, std::size_t min_bytes_each, BinaryR
|
||||
return trace;
|
||||
}
|
||||
|
||||
// The capture windows live in a TRAILER after the trace blocks rather than inside
|
||||
// them, so a reader built before they existed still decodes every trace and simply
|
||||
// stops early. The trailer grows the same way: collection window first, then the
|
||||
// per-trace window table (one pair per trace, in trace order).
|
||||
// Capture trailer: collection window, then one per-trace window in trace order.
|
||||
void write_trace_collection(BinaryWriter& writer, std::uint32_t magic, const RawSweepCollection& collection) {
|
||||
writer.write(magic);
|
||||
writer.write(collection.collection_id);
|
||||
@@ -214,22 +189,9 @@ void write_trace_collection(BinaryWriter& writer, std::uint32_t magic, const Raw
|
||||
collection.traces.push_back(read_trace_block(reader));
|
||||
}
|
||||
|
||||
// Each trailer stage is optional: a payload from an older producer stops after
|
||||
// the trace blocks (or after the collection window) and leaves the rest zeroed.
|
||||
if (reader.remaining_bytes() == 0U) {
|
||||
return collection;
|
||||
}
|
||||
if (reader.remaining_bytes() < (sizeof(std::uint64_t) * 2U)) {
|
||||
throw std::runtime_error("Truncated capture window in trace collection");
|
||||
}
|
||||
|
||||
collection.capture_start_ns = reader.read<std::uint64_t>();
|
||||
collection.capture_end_ns = reader.read<std::uint64_t>();
|
||||
|
||||
if (reader.remaining_bytes() == 0U) {
|
||||
return collection;
|
||||
}
|
||||
|
||||
const auto trace_time_count = reader.read<std::uint32_t>();
|
||||
if (trace_time_count != collection.traces.size()) {
|
||||
throw std::runtime_error("Per-trace capture window count does not match trace count");
|
||||
@@ -255,23 +217,6 @@ void write_trace_result_payload(BinaryWriter& writer, const ResultPayload& paylo
|
||||
}
|
||||
}
|
||||
|
||||
auto read_trace_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
|
||||
const auto point_count = reader.read<std::uint32_t>();
|
||||
|
||||
payload->frequency_hz.reserve(point_count);
|
||||
payload->trace.reserve(point_count);
|
||||
|
||||
for (std::uint32_t index = 0; index < point_count; ++index) {
|
||||
payload->frequency_hz.push_back(reader.read<float>());
|
||||
}
|
||||
for (std::uint32_t index = 0; index < point_count; ++index) {
|
||||
payload->trace.push_back(Complex32{
|
||||
.re = reader.read<float>(),
|
||||
.im = reader.read<float>(),
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void write_image_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
|
||||
const auto x_count = checked_count_to_u32(payload.image_x_axis.size(), "Result image x axis point count");
|
||||
const auto y_count = checked_count_to_u32(payload.image_y_axis.size(), "Result image y axis point count");
|
||||
@@ -293,25 +238,6 @@ void write_image_result_payload(BinaryWriter& writer, const ResultPayload& paylo
|
||||
}
|
||||
}
|
||||
|
||||
auto read_image_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
|
||||
const auto x_count = reader.read<std::uint32_t>();
|
||||
const auto y_count = reader.read<std::uint32_t>();
|
||||
|
||||
payload->image_x_axis.reserve(x_count);
|
||||
payload->image_y_axis.reserve(y_count);
|
||||
payload->image_values.reserve(static_cast<std::size_t>(x_count) * static_cast<std::size_t>(y_count));
|
||||
|
||||
for (std::uint32_t index = 0; index < x_count; ++index) {
|
||||
payload->image_x_axis.push_back(reader.read<float>());
|
||||
}
|
||||
for (std::uint32_t index = 0; index < y_count; ++index) {
|
||||
payload->image_y_axis.push_back(reader.read<float>());
|
||||
}
|
||||
for (std::size_t index = 0; index < static_cast<std::size_t>(x_count) * static_cast<std::size_t>(y_count); ++index) {
|
||||
payload->image_values.push_back(reader.read<float>());
|
||||
}
|
||||
}
|
||||
|
||||
void write_table_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
|
||||
const auto column_count = payload.table_columns;
|
||||
if (column_count == 0U && !payload.table_values.empty()) {
|
||||
@@ -331,16 +257,6 @@ void write_table_result_payload(BinaryWriter& writer, const ResultPayload& paylo
|
||||
}
|
||||
}
|
||||
|
||||
auto read_table_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
|
||||
const auto column_count = reader.read<std::uint32_t>();
|
||||
const auto row_count = reader.read<std::uint32_t>();
|
||||
payload->table_columns = column_count;
|
||||
payload->table_values.reserve(static_cast<std::size_t>(column_count) * static_cast<std::size_t>(row_count));
|
||||
for (std::size_t index = 0; index < static_cast<std::size_t>(column_count) * static_cast<std::size_t>(row_count); ++index) {
|
||||
payload->table_values.push_back(reader.read<float>());
|
||||
}
|
||||
}
|
||||
|
||||
void write_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
|
||||
writer.write(static_cast<std::uint8_t>(payload.kind));
|
||||
writer.write_string(payload.processing_name);
|
||||
@@ -363,29 +279,6 @@ void write_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_result_payload(BinaryReader& reader) -> ResultPayload {
|
||||
ResultPayload payload{};
|
||||
payload.kind = static_cast<ResultKind>(reader.read<std::uint8_t>());
|
||||
payload.processing_name = reader.read_string();
|
||||
|
||||
switch (payload.kind) {
|
||||
case ResultKind::TraceComplex:
|
||||
read_trace_result_payload(reader, &payload);
|
||||
return payload;
|
||||
case ResultKind::ScalarF32:
|
||||
payload.scalar_value = reader.read<float>();
|
||||
return payload;
|
||||
case ResultKind::ImageF32:
|
||||
read_image_result_payload(reader, &payload);
|
||||
return payload;
|
||||
case ResultKind::TableF32:
|
||||
read_table_result_payload(reader, &payload);
|
||||
return payload;
|
||||
default:
|
||||
throw std::runtime_error("Unsupported result payload kind in stream");
|
||||
}
|
||||
}
|
||||
|
||||
void write_result_block(BinaryWriter& writer, const ResultBlock& block) {
|
||||
writer.write(block.combo.input_pos);
|
||||
writer.write(block.combo.output_pos);
|
||||
@@ -396,20 +289,6 @@ void write_result_block(BinaryWriter& writer, const ResultBlock& block) {
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_result_block(BinaryReader& reader) -> ResultBlock {
|
||||
ResultBlock block{};
|
||||
block.combo.input_pos = reader.read<std::uint32_t>();
|
||||
block.combo.output_pos = reader.read<std::uint32_t>();
|
||||
|
||||
const auto payload_count = reader.read<std::uint32_t>();
|
||||
block.payloads.reserve(payload_count);
|
||||
for (std::uint32_t index = 0; index < payload_count; ++index) {
|
||||
block.payloads.push_back(read_result_payload(reader));
|
||||
}
|
||||
|
||||
return block;
|
||||
}
|
||||
|
||||
void ensure_reader_consumed(const BinaryReader& reader, const std::string& payload_label) {
|
||||
if (!reader.is_consumed()) {
|
||||
throw std::runtime_error("Unexpected trailing bytes in " + payload_label);
|
||||
@@ -450,6 +329,10 @@ auto serialize_result_collection(const ResultCollection& collection) -> std::vec
|
||||
writer.write(collection.collection_id);
|
||||
writer.write(collection.monotonic_ns);
|
||||
writer.write(collection.processing_duration_ns);
|
||||
writer.write(collection.settings_revision);
|
||||
writer.write(collection.capture_start_ns);
|
||||
writer.write(collection.capture_end_ns);
|
||||
writer.write(collection.speed_m_s);
|
||||
writer.write(checked_count_to_u32(collection.collection_payloads.size(), "Collection payload count"));
|
||||
writer.write(checked_count_to_u32(collection.blocks.size(), "Result block count"));
|
||||
|
||||
@@ -463,35 +346,6 @@ auto serialize_result_collection(const ResultCollection& collection) -> std::vec
|
||||
return std::move(writer).finish();
|
||||
}
|
||||
|
||||
auto deserialize_result_collection(std::span<const std::uint8_t> bytes) -> ResultCollection {
|
||||
BinaryReader reader(bytes);
|
||||
|
||||
const auto magic = reader.read<std::uint32_t>();
|
||||
if (magic != kResultCollectionMagic) {
|
||||
throw std::runtime_error("Unexpected result collection magic");
|
||||
}
|
||||
|
||||
ResultCollection collection{};
|
||||
collection.collection_id = reader.read<std::uint64_t>();
|
||||
collection.monotonic_ns = reader.read<std::uint64_t>();
|
||||
collection.processing_duration_ns = reader.read<std::uint64_t>();
|
||||
|
||||
const auto collection_payload_count = reader.read<std::uint32_t>();
|
||||
const auto block_count = reader.read<std::uint32_t>();
|
||||
collection.collection_payloads.reserve(collection_payload_count);
|
||||
for (std::uint32_t index = 0; index < collection_payload_count; ++index) {
|
||||
collection.collection_payloads.push_back(read_result_payload(reader));
|
||||
}
|
||||
|
||||
collection.blocks.reserve(block_count);
|
||||
for (std::uint32_t index = 0; index < block_count; ++index) {
|
||||
collection.blocks.push_back(read_result_block(reader));
|
||||
}
|
||||
|
||||
ensure_reader_consumed(reader, "result collection");
|
||||
return collection;
|
||||
}
|
||||
|
||||
auto current_monotonic_ns() -> std::uint64_t {
|
||||
const auto now = std::chrono::steady_clock::now().time_since_epoch();
|
||||
const auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now).count();
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cerrno>
|
||||
@@ -30,7 +31,7 @@ struct alignas(64) ShmRing::Header {
|
||||
std::atomic<std::uint64_t> dropped{};
|
||||
};
|
||||
|
||||
struct alignas(16) ShmRing::SlotHeader {
|
||||
struct alignas(8) ShmRing::SlotHeader {
|
||||
std::uint32_t payload_size = 0;
|
||||
std::uint32_t reserved = 0;
|
||||
std::uint64_t sequence = 0;
|
||||
@@ -42,64 +43,12 @@ constexpr std::string_view kRingMagic = "RDRRING2";
|
||||
constexpr std::uint32_t kRingVersion = 1;
|
||||
constexpr auto kHeaderInitWaitTimeout = std::chrono::milliseconds(1000);
|
||||
constexpr auto kHeaderInitPollInterval = std::chrono::milliseconds(2);
|
||||
|
||||
[[nodiscard]] auto checked_u64_diff(std::uint64_t left, std::uint64_t right) -> std::uint64_t {
|
||||
return left >= right ? left - right : 0;
|
||||
}
|
||||
constexpr std::uint64_t kMaxMappedSize = 1ULL << 40U;
|
||||
|
||||
[[nodiscard]] auto errno_message(const std::string& action, const std::string& name) -> std::runtime_error {
|
||||
return std::runtime_error(action + " " + name + ": " + std::strerror(errno));
|
||||
}
|
||||
|
||||
class ScopedFd {
|
||||
public:
|
||||
explicit ScopedFd(int fd) : fd_(fd) {}
|
||||
|
||||
~ScopedFd() {
|
||||
if (fd_ >= 0) {
|
||||
::close(fd_);
|
||||
}
|
||||
}
|
||||
|
||||
ScopedFd(const ScopedFd&) = delete;
|
||||
auto operator=(const ScopedFd&) -> ScopedFd& = delete;
|
||||
|
||||
ScopedFd(ScopedFd&& other) noexcept : fd_(std::exchange(other.fd_, -1)) {}
|
||||
|
||||
auto release() -> int {
|
||||
return std::exchange(fd_, -1);
|
||||
}
|
||||
|
||||
private:
|
||||
int fd_ = -1;
|
||||
};
|
||||
|
||||
class ScopedMmap {
|
||||
public:
|
||||
ScopedMmap(void* address, std::size_t size) : address_(address), size_(size) {}
|
||||
|
||||
~ScopedMmap() {
|
||||
if (address_ != nullptr && address_ != MAP_FAILED) {
|
||||
munmap(address_, size_);
|
||||
}
|
||||
}
|
||||
|
||||
ScopedMmap(const ScopedMmap&) = delete;
|
||||
auto operator=(const ScopedMmap&) -> ScopedMmap& = delete;
|
||||
|
||||
ScopedMmap(ScopedMmap&& other) noexcept
|
||||
: address_(std::exchange(other.address_, nullptr)),
|
||||
size_(std::exchange(other.size_, 0U)) {}
|
||||
|
||||
auto release() -> std::pair<void*, std::size_t> {
|
||||
return {std::exchange(address_, nullptr), std::exchange(size_, 0U)};
|
||||
}
|
||||
|
||||
private:
|
||||
void* address_ = nullptr;
|
||||
std::size_t size_ = 0;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
ShmRing::~ShmRing() {
|
||||
@@ -128,12 +77,7 @@ auto ShmRing::open_or_create(
|
||||
std::uint32_t slot_size_bytes
|
||||
) -> ShmRing {
|
||||
validate_name(name);
|
||||
if (capacity == 0U) {
|
||||
throw std::runtime_error("Ring capacity must be > 0");
|
||||
}
|
||||
if (slot_size_bytes == 0U) {
|
||||
throw std::runtime_error("Ring slot size must be > 0");
|
||||
}
|
||||
const auto mapped_size = checked_mapped_size(capacity, slot_size_bytes, name);
|
||||
|
||||
bool created = false;
|
||||
int fd = shm_open(name.c_str(), O_RDWR | O_CREAT | O_EXCL, 0660);
|
||||
@@ -147,15 +91,30 @@ auto ShmRing::open_or_create(
|
||||
throw errno_message("Failed to open shared memory ring", name);
|
||||
}
|
||||
|
||||
ScopedFd scoped_fd(fd);
|
||||
|
||||
const auto slot_stride = sizeof(SlotHeader) + static_cast<std::size_t>(slot_size_bytes);
|
||||
const auto mapped_size = sizeof(Header) + slot_stride * capacity;
|
||||
ShmRing ring{};
|
||||
ring.fd_ = fd;
|
||||
|
||||
if (created) {
|
||||
if (ftruncate(fd, static_cast<off_t>(mapped_size)) != 0) {
|
||||
throw errno_message("Failed to resize shared memory ring", name);
|
||||
}
|
||||
} else {
|
||||
// shm_open publishes the name before the creator calls ftruncate. Mapping
|
||||
// that zero/short file and reading its header would terminate us with SIGBUS.
|
||||
const auto deadline = std::chrono::steady_clock::now() + kHeaderInitWaitTimeout;
|
||||
while (true) {
|
||||
struct stat info {};
|
||||
if (fstat(fd, &info) != 0) {
|
||||
throw errno_message("Failed to stat shared memory ring", name);
|
||||
}
|
||||
if (info.st_size >= 0 && static_cast<std::size_t>(info.st_size) >= mapped_size) {
|
||||
break;
|
||||
}
|
||||
if (std::chrono::steady_clock::now() >= deadline) {
|
||||
throw std::runtime_error("Shared memory ring size is too small for " + name);
|
||||
}
|
||||
std::this_thread::sleep_for(kHeaderInitPollInterval);
|
||||
}
|
||||
}
|
||||
|
||||
void* mapped = mmap(nullptr, mapped_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
|
||||
@@ -163,28 +122,29 @@ auto ShmRing::open_or_create(
|
||||
throw errno_message("Failed to mmap shared memory ring", name);
|
||||
}
|
||||
|
||||
ScopedMmap scoped_mmap(mapped, mapped_size);
|
||||
auto* header = static_cast<Header*>(mapped);
|
||||
ring.mapped_ = mapped;
|
||||
ring.mapped_size_ = mapped_size;
|
||||
auto* header = ring.header_ = static_cast<Header*>(mapped);
|
||||
|
||||
if (created) {
|
||||
std::memset(mapped, 0, mapped_size);
|
||||
// ftruncate already supplies zero-filled slots; leave them demand-paged
|
||||
// instead of touching the full ring before any payload is published.
|
||||
std::memset(mapped, 0, sizeof(Header));
|
||||
std::memcpy(header->magic, kRingMagic.data(), kRingMagic.size());
|
||||
header->version = kRingVersion;
|
||||
header->capacity = capacity;
|
||||
header->slot_size_bytes = slot_size_bytes;
|
||||
|
||||
new (&header->write_seq) std::atomic<std::uint64_t>(0);
|
||||
new (&header->read_seq) std::atomic<std::uint64_t>(0);
|
||||
new (&header->dropped) std::atomic<std::uint64_t>(0);
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
// Publish readiness only after geometry and cursors have been initialized.
|
||||
std::atomic_ref(header->version).store(kRingVersion, std::memory_order_release);
|
||||
} else {
|
||||
const auto deadline = std::chrono::steady_clock::now() + kHeaderInitWaitTimeout;
|
||||
|
||||
while (true) {
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
|
||||
const bool version_ok = std::atomic_ref(header->version).load(std::memory_order_acquire) == kRingVersion;
|
||||
const bool magic_ok = std::memcmp(header->magic, kRingMagic.data(), kRingMagic.size()) == 0;
|
||||
const bool version_ok = header->version == kRingVersion;
|
||||
const bool geometry_ok = header->capacity == capacity && header->slot_size_bytes == slot_size_bytes;
|
||||
|
||||
if (magic_ok && version_ok && geometry_ok) {
|
||||
@@ -213,89 +173,13 @@ auto ShmRing::open_or_create(
|
||||
}
|
||||
}
|
||||
|
||||
ShmRing ring{};
|
||||
ring.fd_ = scoped_fd.release();
|
||||
|
||||
const auto [released_mapped, released_size] = scoped_mmap.release();
|
||||
ring.mapped_ = released_mapped;
|
||||
ring.mapped_size_ = released_size;
|
||||
ring.header_ = static_cast<Header*>(released_mapped);
|
||||
return ring;
|
||||
}
|
||||
|
||||
auto ShmRing::open_existing(const std::string& name) -> ShmRing {
|
||||
validate_name(name);
|
||||
|
||||
const int fd = shm_open(name.c_str(), O_RDWR, 0660);
|
||||
if (fd < 0) {
|
||||
throw errno_message("Failed to open shared memory ring", name);
|
||||
}
|
||||
|
||||
ScopedFd scoped_fd(fd);
|
||||
|
||||
struct stat info {};
|
||||
if (fstat(fd, &info) != 0) {
|
||||
throw errno_message("Failed to stat shared memory ring", name);
|
||||
}
|
||||
|
||||
if (info.st_size < static_cast<off_t>(sizeof(Header))) {
|
||||
throw std::runtime_error("Shared memory ring size is too small for " + name);
|
||||
}
|
||||
|
||||
const auto mapped_size = static_cast<std::size_t>(info.st_size);
|
||||
void* mapped = mmap(nullptr, mapped_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
|
||||
if (mapped == MAP_FAILED) {
|
||||
throw errno_message("Failed to mmap shared memory ring", name);
|
||||
}
|
||||
|
||||
ScopedMmap scoped_mmap(mapped, mapped_size);
|
||||
auto* header = static_cast<Header*>(mapped);
|
||||
|
||||
if (std::memcmp(header->magic, kRingMagic.data(), kRingMagic.size()) != 0) {
|
||||
throw std::runtime_error("Shared memory ring magic mismatch for " + name);
|
||||
}
|
||||
if (header->version != kRingVersion) {
|
||||
throw std::runtime_error("Shared memory ring version mismatch for " + name);
|
||||
}
|
||||
// ensure the mapping actually spans every slot the header describes.
|
||||
validate_geometry(*header, mapped_size, name);
|
||||
|
||||
ShmRing ring{};
|
||||
ring.fd_ = scoped_fd.release();
|
||||
|
||||
const auto [released_mapped, released_size] = scoped_mmap.release();
|
||||
ring.mapped_ = released_mapped;
|
||||
ring.mapped_size_ = released_size;
|
||||
ring.header_ = static_cast<Header*>(released_mapped);
|
||||
return ring;
|
||||
}
|
||||
|
||||
void ShmRing::unlink_ring(const std::string& name) {
|
||||
validate_name(name);
|
||||
if (shm_unlink(name.c_str()) != 0 && errno != ENOENT) {
|
||||
throw errno_message("Failed to unlink shared memory ring", name);
|
||||
}
|
||||
}
|
||||
|
||||
auto ShmRing::is_open() const -> bool {
|
||||
return header_ != nullptr;
|
||||
}
|
||||
|
||||
auto ShmRing::capacity() const -> std::uint32_t {
|
||||
return header_ != nullptr ? header_->capacity : 0;
|
||||
}
|
||||
|
||||
auto ShmRing::slot_size_bytes() const -> std::uint32_t {
|
||||
return header_ != nullptr ? header_->slot_size_bytes : 0;
|
||||
}
|
||||
|
||||
auto ShmRing::dropped_count() const -> std::uint64_t {
|
||||
if (header_ == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
return header_->dropped.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
|
||||
if (header_ == nullptr) {
|
||||
throw std::runtime_error("Ring is not open");
|
||||
@@ -307,7 +191,7 @@ auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
|
||||
const auto write_seq = header_->write_seq.load(std::memory_order_relaxed);
|
||||
auto read_seq = header_->read_seq.load(std::memory_order_acquire);
|
||||
|
||||
if (checked_u64_diff(write_seq, read_seq) >= header_->capacity) {
|
||||
if (write_seq >= read_seq && write_seq - read_seq >= header_->capacity) {
|
||||
// Ring full: overwrite the oldest unread slot. Advance read_seq via CAS so a
|
||||
// concurrent consumer's advance is never clobbered (read_seq never moves back).
|
||||
header_->read_seq.compare_exchange_strong(
|
||||
@@ -319,14 +203,13 @@ auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
|
||||
}
|
||||
|
||||
auto* slot = slot_header(write_seq);
|
||||
// Seqlock publish: write the payload + size FIRST, then store the slot sequence
|
||||
// LAST (after a release fence). A consumer that observes sequence == write_seq+1
|
||||
// is therefore guaranteed a fully-written payload. (Publishing the sequence before
|
||||
// the copy, as before, let a consumer copy a half-written slot.)
|
||||
// Invalidate the old publication before overwriting its bytes: a slow consumer
|
||||
// can otherwise accept a mixture of old and new data under the old sequence.
|
||||
std::atomic_ref(slot->sequence).store(0U, std::memory_order_relaxed);
|
||||
std::atomic_thread_fence(std::memory_order_seq_cst);
|
||||
slot->payload_size = static_cast<std::uint32_t>(payload.size());
|
||||
std::memcpy(slot_payload(slot), payload.data(), payload.size());
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
slot->sequence = write_seq + 1U;
|
||||
std::atomic_ref(slot->sequence).store(write_seq + 1U, std::memory_order_release);
|
||||
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
header_->write_seq.store(write_seq + 1U, std::memory_order_release);
|
||||
@@ -338,41 +221,49 @@ auto ShmRing::pop(std::vector<std::uint8_t>& payload) -> bool {
|
||||
throw std::runtime_error("Ring is not open");
|
||||
}
|
||||
|
||||
const auto read_seq = header_->read_seq.load(std::memory_order_relaxed);
|
||||
const auto write_seq = header_->write_seq.load(std::memory_order_acquire);
|
||||
if (read_seq >= write_seq) {
|
||||
return false;
|
||||
while (true) {
|
||||
const auto read_seq = header_->read_seq.load(std::memory_order_relaxed);
|
||||
const auto write_seq = header_->write_seq.load(std::memory_order_acquire);
|
||||
if (read_seq >= write_seq) {
|
||||
return false;
|
||||
}
|
||||
// A lapped, torn or corrupt slot is skipped with the backlog behind the newest
|
||||
// payload, which is read next (latest-wins); a bad slot never stops the consumer.
|
||||
const auto resync = std::max(read_seq + 1U, write_seq - 1U);
|
||||
|
||||
auto* slot = slot_header(read_seq);
|
||||
if (std::atomic_ref(slot->sequence).load(std::memory_order_acquire) != read_seq + 1U) {
|
||||
advance_read_seq(resync); // Overwritten before this read (lapped).
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto payload_size = slot->payload_size;
|
||||
if (payload_size > header_->slot_size_bytes) {
|
||||
advance_read_seq(resync);
|
||||
continue;
|
||||
}
|
||||
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
payload.resize(payload_size);
|
||||
std::memcpy(payload.data(), slot_payload(slot), payload_size);
|
||||
|
||||
// Seqlock verify: if the slot sequence changed during the copy, the producer
|
||||
// lapped us mid-read and the payload is torn.
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
if (std::atomic_ref(slot->sequence).load(std::memory_order_acquire) != read_seq + 1U) {
|
||||
advance_read_seq(resync);
|
||||
continue;
|
||||
}
|
||||
|
||||
advance_read_seq(read_seq + 1U);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
auto* slot = slot_header(read_seq);
|
||||
if (slot->sequence != read_seq + 1U) {
|
||||
// Slot not published for this seq, or already overwritten (lapped). Resync.
|
||||
header_->read_seq.store(write_seq, std::memory_order_release);
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto payload_size = slot->payload_size;
|
||||
if (payload_size > header_->slot_size_bytes) {
|
||||
// Corrupt/torn slot: skip it (resync) rather than throw — a single bad slot
|
||||
// must never abort the long-running consumer.
|
||||
header_->read_seq.store(write_seq, std::memory_order_release);
|
||||
return false;
|
||||
}
|
||||
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
payload.resize(payload_size);
|
||||
std::memcpy(payload.data(), slot_payload(slot), payload_size);
|
||||
|
||||
// Seqlock verify: if the slot sequence changed during the copy, the producer
|
||||
// lapped us mid-read and the payload is torn — discard and resync.
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
if (slot->sequence != read_seq + 1U) {
|
||||
header_->read_seq.store(write_seq, std::memory_order_release);
|
||||
return false;
|
||||
}
|
||||
|
||||
header_->read_seq.store(read_seq + 1U, std::memory_order_release);
|
||||
return true;
|
||||
void ShmRing::advance_read_seq(std::uint64_t target) {
|
||||
auto current = header_->read_seq.load(std::memory_order_relaxed);
|
||||
while (current < target && !header_->read_seq.compare_exchange_weak(
|
||||
current, target, std::memory_order_release, std::memory_order_relaxed)) {}
|
||||
}
|
||||
|
||||
auto ShmRing::slot_stride_bytes() const -> std::size_t {
|
||||
@@ -405,37 +296,41 @@ void ShmRing::close() {
|
||||
}
|
||||
|
||||
void ShmRing::validate_name(const std::string& name) {
|
||||
if (name.empty() || name.front() != '/') {
|
||||
throw std::runtime_error("POSIX shm name must start with '/'");
|
||||
if (name.size() < 2U || name.front() != '/' || name.find('/', 1) != std::string::npos
|
||||
|| name.find('\0') != std::string::npos) {
|
||||
throw std::runtime_error("POSIX shm name must be '/' followed by a single nonempty filename");
|
||||
}
|
||||
}
|
||||
|
||||
void ShmRing::validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name) {
|
||||
// derive the expected size from the header's own geometry fields and
|
||||
// require the real mapping to cover it. A bogus capacity/slot_size or a truncated
|
||||
// mapping would otherwise yield out-of-bounds slot offsets and a SIGSEGV.
|
||||
const std::uint32_t capacity = header.capacity;
|
||||
const std::uint32_t slot_size_bytes = header.slot_size_bytes;
|
||||
auto ShmRing::checked_mapped_size(
|
||||
std::uint32_t capacity, std::uint32_t slot_size_bytes, const std::string& name
|
||||
) -> std::size_t {
|
||||
static_assert(sizeof(Header) == 64 && sizeof(SlotHeader) == 16);
|
||||
if (capacity == 0U) {
|
||||
throw std::runtime_error("Shared memory ring capacity is zero for " + name);
|
||||
throw std::runtime_error("Ring capacity must be > 0");
|
||||
}
|
||||
if (slot_size_bytes == 0U) {
|
||||
throw std::runtime_error("Shared memory ring slot size is zero for " + name);
|
||||
throw std::runtime_error("Ring slot size must be > 0");
|
||||
}
|
||||
if (slot_size_bytes % alignof(SlotHeader) != 0U) {
|
||||
throw std::runtime_error("Ring slot size must be a multiple of 8");
|
||||
}
|
||||
|
||||
constexpr auto kMax = std::numeric_limits<std::size_t>::max();
|
||||
const auto slot_stride = sizeof(SlotHeader) + static_cast<std::size_t>(slot_size_bytes);
|
||||
// Guard each multiply/add against std::size_t overflow before computing expected_size.
|
||||
if (slot_stride > kMax / static_cast<std::size_t>(capacity)) {
|
||||
const auto stride = sizeof(SlotHeader) + static_cast<std::uint64_t>(slot_size_bytes);
|
||||
if (stride > (kMaxMappedSize - sizeof(Header)) / capacity) {
|
||||
throw std::runtime_error("Shared memory ring exceeds 1 TiB for " + name);
|
||||
}
|
||||
const auto mapped_size_u64 = sizeof(Header) + stride * capacity;
|
||||
if (mapped_size_u64 > kMaxMappedSize
|
||||
|| mapped_size_u64 > std::numeric_limits<std::size_t>::max()
|
||||
|| mapped_size_u64 > static_cast<std::uint64_t>(std::numeric_limits<off_t>::max())) {
|
||||
throw std::runtime_error("Shared memory ring geometry overflows for " + name);
|
||||
}
|
||||
const auto slots_size = slot_stride * static_cast<std::size_t>(capacity);
|
||||
if (slots_size > kMax - sizeof(Header)) {
|
||||
throw std::runtime_error("Shared memory ring geometry overflows for " + name);
|
||||
}
|
||||
const auto expected_size = sizeof(Header) + slots_size;
|
||||
return static_cast<std::size_t>(mapped_size_u64);
|
||||
}
|
||||
|
||||
if (mapped_size < expected_size) {
|
||||
void ShmRing::validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name) {
|
||||
if (mapped_size < checked_mapped_size(header.capacity, header.slot_size_bytes, name)) {
|
||||
throw std::runtime_error("Shared memory ring size is too small for " + name);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,48 +0,0 @@
|
||||
# Kamil ADC collector
|
||||
|
||||
Standalone acquisition binary for the **L-Card E-502** ADC. It captures the
|
||||
dual-channel main + reference IQ stream and publishes it as 8-byte framed packets
|
||||
over a PTY, which the Python `kamil_adc` producer
|
||||
(`python_app/hardware_full/kamil_adc`) reads and processes.
|
||||
|
||||
This is the data *source* for `radar.model == "kamil_adc"`; it is the Kamil ADC
|
||||
counterpart to the C++ `sweep_orchestrator` used by LibreVNA devices.
|
||||
|
||||
## Layout
|
||||
|
||||
| Path | Contents |
|
||||
|------|----------|
|
||||
| `src/` | Collector implementation (`main.cpp`) and the PTY / capture-file writers. |
|
||||
| `include/` | Headers for the writers plus `switch_sequencer.h`. |
|
||||
| `vendor/lcard/` | L-Card X502/E502 SDK headers (`x502api.h`, `e502api.h`, `lcard_pstdint.h`), vendored verbatim so the collector builds with no external include path. |
|
||||
|
||||
The sources started as a verbatim import of the original standalone tool, but are
|
||||
**no longer identical to it**: the collector now drives the RF switches itself
|
||||
(switch-aware mode, `SwitchSequencer`), stepping to the next combination inside
|
||||
the hardware idle gap between sweeps and flagging a sweep *dirty* when a readout
|
||||
backlog means the toggle could not have landed in that gap. It therefore reuses
|
||||
the orchestrator's switch drivers and the shared `run_config` loader, which is why
|
||||
its build has its own include set and its own `build/kamil/` object tree.
|
||||
|
||||
The wire format towards the Python producer is unchanged.
|
||||
|
||||
## Build
|
||||
|
||||
```bash
|
||||
make kamil_adc_collector # -> build/bin/kamil_adc_collector
|
||||
```
|
||||
|
||||
It is **not** part of `make all`; `start.sh` builds it automatically when the
|
||||
active config selects `radar.model == "kamil_adc"`. The build is self-contained:
|
||||
it needs only the vendored headers and links `-ldl -lutil -lpthread`.
|
||||
|
||||
## Runtime
|
||||
|
||||
The collector loads the proprietary `libx502api.so` / `libe502api.so` at run time
|
||||
via `dlopen`, so those libraries must be on `LD_LIBRARY_PATH`. The Kamil ADC
|
||||
service prepends `~/.local/lib` (their default install location) unless the config
|
||||
pins `LD_LIBRARY_PATH` in `radar.kamil_adc.env`.
|
||||
|
||||
The command-line arguments (capture profile, clocking, the `do8_freq_ref`
|
||||
reference overlay, etc.) are supplied by `radar.kamil_adc.args` in the run config;
|
||||
the service appends the generated `tty:<path>` argument.
|
||||
@@ -1,55 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
struct CapturePacket {
|
||||
std::size_t packet_index = 0;
|
||||
std::size_t channel_count = 2;
|
||||
bool has_di1_trace = false;
|
||||
std::vector<double> ch1;
|
||||
std::vector<double> ch2;
|
||||
std::vector<uint8_t> di1;
|
||||
};
|
||||
|
||||
class CaptureFileWriter {
|
||||
public:
|
||||
CaptureFileWriter(std::string csv_path,
|
||||
std::string svg_path,
|
||||
std::string live_html_path,
|
||||
std::string live_json_path);
|
||||
|
||||
void write(const std::vector<CapturePacket>& packets,
|
||||
double frame_freq_hz,
|
||||
double nominal_range_v) const;
|
||||
|
||||
void initialize_live_plot() const;
|
||||
void initialize_csv(std::size_t channel_count, bool has_di1_trace) const;
|
||||
void append_csv_packet(const CapturePacket& packet,
|
||||
double frame_freq_hz,
|
||||
std::size_t& global_frame_index) const;
|
||||
|
||||
void update_live_plot(const CapturePacket& packet,
|
||||
std::size_t packets_seen,
|
||||
double packets_per_second,
|
||||
double frame_freq_hz,
|
||||
const std::string& close_reason,
|
||||
std::size_t zeroed_samples,
|
||||
std::size_t stored_samples) const;
|
||||
|
||||
const std::string& live_html_path() const;
|
||||
const std::string& live_json_path() const;
|
||||
|
||||
private:
|
||||
void finalize_csv_from_spool(double frame_freq_hz) const;
|
||||
void write_svg(const std::vector<CapturePacket>& packets,
|
||||
double frame_freq_hz,
|
||||
double nominal_range_v) const;
|
||||
|
||||
std::string csv_path_;
|
||||
std::string svg_path_;
|
||||
std::string live_html_path_;
|
||||
std::string live_json_path_;
|
||||
};
|
||||
@@ -1,147 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
#include "switch_driver.hpp"
|
||||
|
||||
namespace radar::kamil {
|
||||
|
||||
/**
|
||||
* @brief Drives the input/output RF switches in lock-step with sweep boundaries.
|
||||
*
|
||||
* The E-502 stream is free-running: sweeps are delimited by DI_SYN2 edges with a
|
||||
* hardware idle gap between them. On every completed sweep we step to the next
|
||||
* switch combination *during that gap*, so the next sweep starts already settled
|
||||
* in the new state and no samples are lost.
|
||||
*
|
||||
* The one failure mode of doing this purely in software is the readout backlog:
|
||||
* if, at the moment we observe a sweep's end, we are running so far behind real
|
||||
* time that the toggle could not have landed within the gap, the upcoming sweep
|
||||
* straddles the transition. We flag that sweep *dirty*; the consumer drops it and
|
||||
* we re-take the same combination on the following sweep (which is then clean,
|
||||
* because no transition happens during its gap). A clean sweep costs nothing; a
|
||||
* miss costs exactly one extra sweep period for that one combination.
|
||||
*
|
||||
* The sequencer owns the two switch drivers and exposes only what the collector
|
||||
* needs at a sweep boundary: which combination the upcoming sweep belongs to and
|
||||
* whether it is expected to be clean. All timing policy lives here so the recv
|
||||
* loop stays readable.
|
||||
*/
|
||||
class SwitchSequencer {
|
||||
public:
|
||||
struct Settings {
|
||||
/// Combinations to cycle through, one sweep each, in order. Empty disables
|
||||
/// the sequencer entirely (the collector then streams a single passthrough
|
||||
/// channel exactly as before).
|
||||
std::vector<radar::ipc::ComboKey> combos{};
|
||||
/// Guaranteed minimum inter-sweep idle window, in milliseconds. This is a
|
||||
/// hardware property of the sweep generator; treat it as a lower bound.
|
||||
double gap_ms = 10.0;
|
||||
/// RF settle time required after a position change, in milliseconds.
|
||||
double settle_ms = 1.0;
|
||||
};
|
||||
|
||||
SwitchSequencer(
|
||||
Settings settings,
|
||||
std::unique_ptr<drivers::SwitchDriver> input_switch,
|
||||
std::unique_ptr<drivers::SwitchDriver> output_switch
|
||||
)
|
||||
: settings_(std::move(settings)),
|
||||
input_switch_(std::move(input_switch)),
|
||||
output_switch_(std::move(output_switch)) {}
|
||||
|
||||
[[nodiscard]] auto enabled() const -> bool { return !settings_.combos.empty(); }
|
||||
|
||||
/// Open both switch drivers and park at the first combination.
|
||||
void open() {
|
||||
if (!enabled()) {
|
||||
return;
|
||||
}
|
||||
output_switch_->open();
|
||||
input_switch_->open();
|
||||
index_ = 0;
|
||||
applied_ = settings_.combos.front();
|
||||
apply(applied_);
|
||||
upcoming_dirty_ = false; // the first sweep is captured in a settled state
|
||||
}
|
||||
|
||||
void close() {
|
||||
if (!enabled()) {
|
||||
return;
|
||||
}
|
||||
// Best-effort: teardown must never throw out of the collector's stop path.
|
||||
try {
|
||||
output_switch_->close();
|
||||
} catch (...) {
|
||||
}
|
||||
try {
|
||||
input_switch_->close();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
/// Combination the in-progress / upcoming sweep is captured under.
|
||||
[[nodiscard]] auto current_combo() const -> radar::ipc::ComboKey {
|
||||
return settings_.combos[index_];
|
||||
}
|
||||
|
||||
/// Whether the in-progress / upcoming sweep is expected to straddle a switch
|
||||
/// transition and must be dropped by the consumer.
|
||||
[[nodiscard]] auto current_dirty() const -> bool { return upcoming_dirty_; }
|
||||
|
||||
/**
|
||||
* @brief Advance the sequence at a completed sweep's end (start of the gap).
|
||||
*
|
||||
* @param backlog_ms Estimated readout lag at this instant — how far behind
|
||||
* real time we are, i.e. how late the toggle we issue now will land.
|
||||
*
|
||||
* If the sweep that just ended was clean we step to the next combination and
|
||||
* toggle the switches now, inside the gap. If it was dirty we re-take the same
|
||||
* combination (the switches are already there and long settled). The upcoming
|
||||
* sweep is marked dirty only when a real transition happens *and* the backlog
|
||||
* leaves no room for it to settle before the next sweep starts.
|
||||
*/
|
||||
void on_sweep_end(double backlog_ms) {
|
||||
if (!enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (upcoming_dirty_) {
|
||||
// The sweep that just ended straddled a transition: re-take the same
|
||||
// combination. The switches are already applied and settled, so the
|
||||
// next sweep is clean without touching the hardware again.
|
||||
upcoming_dirty_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
index_ = (index_ + 1U) % settings_.combos.size();
|
||||
const radar::ipc::ComboKey next = settings_.combos[index_];
|
||||
const bool position_changed = !(next == applied_);
|
||||
if (position_changed) {
|
||||
apply(next);
|
||||
applied_ = next;
|
||||
}
|
||||
upcoming_dirty_ = position_changed && ((backlog_ms + settings_.settle_ms) > settings_.gap_ms);
|
||||
}
|
||||
|
||||
private:
|
||||
void apply(const radar::ipc::ComboKey& combo) {
|
||||
// Output first, then input — same order the sweep orchestrator uses, so
|
||||
// both acquisition paths drive an identical hardware sequence.
|
||||
output_switch_->switch_to(combo.output_pos);
|
||||
input_switch_->switch_to(combo.input_pos);
|
||||
}
|
||||
|
||||
Settings settings_;
|
||||
std::unique_ptr<drivers::SwitchDriver> input_switch_;
|
||||
std::unique_ptr<drivers::SwitchDriver> output_switch_;
|
||||
std::size_t index_ = 0;
|
||||
radar::ipc::ComboKey applied_{};
|
||||
bool upcoming_dirty_ = false;
|
||||
};
|
||||
|
||||
} // namespace radar::kamil
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,450 +0,0 @@
|
||||
#include "tty_protocol_writer.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <exception>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#ifdef _WIN32
|
||||
|
||||
struct TtyProtocolWriter::Impl {};
|
||||
|
||||
TtyProtocolWriter::TtyProtocolWriter(std::string path, std::size_t ring_capacity_bytes)
|
||||
: path_(std::move(path)) {
|
||||
(void) ring_capacity_bytes;
|
||||
throw std::runtime_error("tty output is supported only on Linux/POSIX");
|
||||
}
|
||||
|
||||
TtyProtocolWriter::~TtyProtocolWriter() = default;
|
||||
|
||||
void TtyProtocolWriter::emit_packet_start(uint16_t marker) {
|
||||
(void) marker;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::emit_step(uint16_t index, int16_t ch1_avg, int16_t ch2_avg) {
|
||||
(void) index;
|
||||
(void) ch1_avg;
|
||||
(void) ch2_avg;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::enqueue_encoded_frames(const uint16_t* words, std::size_t frame_count) {
|
||||
(void) words;
|
||||
(void) frame_count;
|
||||
}
|
||||
|
||||
TtyProtocolWriter::StatsSnapshot TtyProtocolWriter::stats() const {
|
||||
return {};
|
||||
}
|
||||
|
||||
const std::string& TtyProtocolWriter::path() const {
|
||||
return path_;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::throw_if_failed() const {}
|
||||
|
||||
void TtyProtocolWriter::shutdown() {}
|
||||
|
||||
void TtyProtocolWriter::enqueue_frame(uint16_t word0, uint16_t word1, uint16_t word2, uint16_t word3) {
|
||||
(void) word0;
|
||||
(void) word1;
|
||||
(void) word2;
|
||||
(void) word3;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::worker_loop() {}
|
||||
|
||||
#else
|
||||
|
||||
#include <cerrno>
|
||||
#include <condition_variable>
|
||||
#include <fcntl.h>
|
||||
#include <limits.h>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <pty.h>
|
||||
#include <sstream>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::size_t kFrameWordCount = 4U;
|
||||
constexpr std::size_t kFrameByteCount = kFrameWordCount * sizeof(uint16_t);
|
||||
constexpr std::size_t kWriteBatchFrames = 256U;
|
||||
|
||||
using EncodedFrame = std::array<std::uint8_t, kFrameByteCount>;
|
||||
|
||||
std::string io_error(const std::string& action, const std::string& path) {
|
||||
std::ostringstream out;
|
||||
out << action << " '" << path << "': " << std::strerror(errno);
|
||||
return out.str();
|
||||
}
|
||||
|
||||
void close_fd_if_open(int& fd) noexcept {
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void set_fd_raw(int fd) {
|
||||
struct termios tio {};
|
||||
if (::tcgetattr(fd, &tio) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot read tty attributes for", std::to_string(fd)));
|
||||
}
|
||||
::cfmakeraw(&tio);
|
||||
tio.c_cc[VINTR] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VQUIT] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VERASE] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VKILL] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VEOF] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VTIME] = 0;
|
||||
tio.c_cc[VMIN] = 1;
|
||||
#ifdef VSWTC
|
||||
tio.c_cc[VSWTC] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
tio.c_cc[VSTART] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VSTOP] = _POSIX_VDISABLE;
|
||||
tio.c_cc[VSUSP] = _POSIX_VDISABLE;
|
||||
#ifdef VEOL
|
||||
tio.c_cc[VEOL] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
#ifdef VREPRINT
|
||||
tio.c_cc[VREPRINT] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
#ifdef VDISCARD
|
||||
tio.c_cc[VDISCARD] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
#ifdef VWERASE
|
||||
tio.c_cc[VWERASE] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
#ifdef VLNEXT
|
||||
tio.c_cc[VLNEXT] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
#ifdef VEOL2
|
||||
tio.c_cc[VEOL2] = _POSIX_VDISABLE;
|
||||
#endif
|
||||
if (::tcsetattr(fd, TCSANOW, &tio) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot apply raw tty attributes to", std::to_string(fd)));
|
||||
}
|
||||
}
|
||||
|
||||
bool is_character_device_path(const std::string& path) {
|
||||
struct stat st {};
|
||||
if (::stat(path.c_str(), &st) != 0) {
|
||||
if (errno == ENOENT) {
|
||||
return false;
|
||||
}
|
||||
throw std::runtime_error(io_error("Cannot stat tty output", path));
|
||||
}
|
||||
return S_ISCHR(st.st_mode);
|
||||
}
|
||||
|
||||
std::optional<std::string> read_link_target(const std::string& path) {
|
||||
std::array<char, PATH_MAX> buf {};
|
||||
const ssize_t len = ::readlink(path.c_str(), buf.data(), buf.size() - 1U);
|
||||
if (len < 0) {
|
||||
if (errno == EINVAL || errno == ENOENT) {
|
||||
return std::nullopt;
|
||||
}
|
||||
throw std::runtime_error(io_error("Cannot read symlink", path));
|
||||
}
|
||||
buf[static_cast<std::size_t>(len)] = '\0';
|
||||
return std::string(buf.data());
|
||||
}
|
||||
|
||||
EncodedFrame encode_frame(uint16_t word0, uint16_t word1, uint16_t word2, uint16_t word3) {
|
||||
const uint16_t words[kFrameWordCount] = {word0, word1, word2, word3};
|
||||
EncodedFrame frame {};
|
||||
std::memcpy(frame.data(), words, sizeof(words));
|
||||
return frame;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct TtyProtocolWriter::Impl {
|
||||
explicit Impl(std::size_t ring_capacity_bytes)
|
||||
: capacity_frames(std::max<std::size_t>(1U, ring_capacity_bytes / kFrameByteCount)),
|
||||
ring(capacity_frames) {}
|
||||
|
||||
int fd = -1;
|
||||
int slave_fd = -1;
|
||||
std::string slave_path;
|
||||
bool owns_link = false;
|
||||
|
||||
const std::size_t capacity_frames;
|
||||
std::vector<EncodedFrame> ring;
|
||||
std::size_t head = 0;
|
||||
std::size_t size = 0;
|
||||
|
||||
mutable std::mutex mutex;
|
||||
std::condition_variable data_ready_cv;
|
||||
std::thread worker;
|
||||
bool stop_requested = false;
|
||||
std::exception_ptr failure;
|
||||
StatsSnapshot stats;
|
||||
};
|
||||
|
||||
TtyProtocolWriter::TtyProtocolWriter(std::string path, std::size_t ring_capacity_bytes)
|
||||
: path_(std::move(path)),
|
||||
impl_(std::make_unique<Impl>(ring_capacity_bytes)) {
|
||||
if (is_character_device_path(path_)) {
|
||||
impl_->fd = ::open(path_.c_str(), O_WRONLY | O_NOCTTY);
|
||||
if (impl_->fd < 0) {
|
||||
throw std::runtime_error(io_error("Cannot open tty output", path_));
|
||||
}
|
||||
} else {
|
||||
std::array<char, PATH_MAX> slave_name {};
|
||||
if (::openpty(&impl_->fd, &impl_->slave_fd, slave_name.data(), nullptr, nullptr) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot create PTY bridge for", path_));
|
||||
}
|
||||
|
||||
try {
|
||||
impl_->slave_path = slave_name.data();
|
||||
set_fd_raw(impl_->slave_fd);
|
||||
|
||||
struct stat st {};
|
||||
if (::lstat(path_.c_str(), &st) == 0) {
|
||||
if (!S_ISLNK(st.st_mode) && !S_ISREG(st.st_mode)) {
|
||||
throw std::runtime_error("Refusing to replace non-link path '" + path_ + "'");
|
||||
}
|
||||
if (::unlink(path_.c_str()) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot remove existing tty link", path_));
|
||||
}
|
||||
} else if (errno != ENOENT) {
|
||||
throw std::runtime_error(io_error("Cannot inspect tty link path", path_));
|
||||
}
|
||||
|
||||
if (::symlink(impl_->slave_path.c_str(), path_.c_str()) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot create tty symlink", path_));
|
||||
}
|
||||
impl_->owns_link = true;
|
||||
} catch (...) {
|
||||
close_fd_if_open(impl_->slave_fd);
|
||||
close_fd_if_open(impl_->fd);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
impl_->worker = std::thread([this]() { worker_loop(); });
|
||||
}
|
||||
|
||||
TtyProtocolWriter::~TtyProtocolWriter() {
|
||||
try {
|
||||
shutdown();
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (impl_->owns_link && !path_.empty()) {
|
||||
try {
|
||||
const auto target = read_link_target(path_);
|
||||
if (target && (*target == impl_->slave_path)) {
|
||||
::unlink(path_.c_str());
|
||||
}
|
||||
} catch (...) {
|
||||
}
|
||||
impl_->owns_link = false;
|
||||
}
|
||||
|
||||
close_fd_if_open(impl_->slave_fd);
|
||||
close_fd_if_open(impl_->fd);
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::emit_packet_start(uint16_t marker) {
|
||||
enqueue_frame(marker, 0xFFFF, 0xFFFF, 0xFFFF);
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::emit_step(uint16_t index, int16_t ch1_avg, int16_t ch2_avg) {
|
||||
enqueue_frame(0x000A,
|
||||
index,
|
||||
static_cast<uint16_t>(ch1_avg),
|
||||
static_cast<uint16_t>(ch2_avg));
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::enqueue_encoded_frames(const uint16_t* words, std::size_t frame_count) {
|
||||
if ((frame_count == 0U) || (words == nullptr)) {
|
||||
return;
|
||||
}
|
||||
|
||||
throw_if_failed();
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (impl_->failure) {
|
||||
std::rethrow_exception(impl_->failure);
|
||||
}
|
||||
if (impl_->stop_requested) {
|
||||
throw std::runtime_error("tty writer is already shut down");
|
||||
}
|
||||
|
||||
std::size_t start_frame = 0;
|
||||
std::size_t frames_to_copy = frame_count;
|
||||
std::size_t dropped_frames = 0;
|
||||
|
||||
if (frame_count >= impl_->capacity_frames) {
|
||||
start_frame = frame_count - impl_->capacity_frames;
|
||||
frames_to_copy = impl_->capacity_frames;
|
||||
dropped_frames = impl_->size + start_frame;
|
||||
impl_->head = 0;
|
||||
impl_->size = 0;
|
||||
} else {
|
||||
const std::size_t available_frames = impl_->capacity_frames - impl_->size;
|
||||
if (frame_count > available_frames) {
|
||||
dropped_frames = frame_count - available_frames;
|
||||
impl_->head = (impl_->head + dropped_frames) % impl_->capacity_frames;
|
||||
impl_->size -= dropped_frames;
|
||||
}
|
||||
}
|
||||
|
||||
if (dropped_frames != 0U) {
|
||||
impl_->stats.frames_dropped += static_cast<std::uint64_t>(dropped_frames);
|
||||
++impl_->stats.ring_overflows;
|
||||
}
|
||||
|
||||
for (std::size_t i = 0; i < frames_to_copy; ++i) {
|
||||
const std::size_t src_index = (start_frame + i) * kFrameWordCount;
|
||||
const std::size_t dst_index = (impl_->head + impl_->size) % impl_->capacity_frames;
|
||||
impl_->ring[dst_index] = encode_frame(words[src_index + 0U],
|
||||
words[src_index + 1U],
|
||||
words[src_index + 2U],
|
||||
words[src_index + 3U]);
|
||||
++impl_->size;
|
||||
}
|
||||
|
||||
impl_->data_ready_cv.notify_one();
|
||||
}
|
||||
|
||||
TtyProtocolWriter::StatsSnapshot TtyProtocolWriter::stats() const {
|
||||
if (!impl_) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
return impl_->stats;
|
||||
}
|
||||
|
||||
const std::string& TtyProtocolWriter::path() const {
|
||||
return path_;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::throw_if_failed() const {
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::exception_ptr failure;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
failure = impl_->failure;
|
||||
}
|
||||
|
||||
if (failure) {
|
||||
std::rethrow_exception(failure);
|
||||
}
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::shutdown() {
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
impl_->stop_requested = true;
|
||||
}
|
||||
|
||||
close_fd_if_open(impl_->fd);
|
||||
impl_->data_ready_cv.notify_all();
|
||||
|
||||
if (impl_->worker.joinable()) {
|
||||
impl_->worker.join();
|
||||
}
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::enqueue_frame(uint16_t word0, uint16_t word1, uint16_t word2, uint16_t word3) {
|
||||
const uint16_t words[kFrameWordCount] = {word0, word1, word2, word3};
|
||||
enqueue_encoded_frames(words, 1U);
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::worker_loop() {
|
||||
for (;;) {
|
||||
std::array<std::uint8_t, kWriteBatchFrames * kFrameByteCount> write_batch;
|
||||
std::size_t batch_count = 0;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(impl_->mutex);
|
||||
impl_->data_ready_cv.wait(lock, [this]() {
|
||||
return impl_->stop_requested || impl_->failure || (impl_->size != 0U);
|
||||
});
|
||||
|
||||
if (impl_->failure || impl_->stop_requested) {
|
||||
return;
|
||||
}
|
||||
|
||||
batch_count = std::min<std::size_t>(impl_->size, kWriteBatchFrames);
|
||||
for (std::size_t i = 0; i < batch_count; ++i) {
|
||||
const EncodedFrame& frame = impl_->ring[impl_->head];
|
||||
std::memcpy(write_batch.data() + (i * kFrameByteCount), frame.data(), kFrameByteCount);
|
||||
impl_->head = (impl_->head + 1U) % impl_->capacity_frames;
|
||||
--impl_->size;
|
||||
}
|
||||
}
|
||||
|
||||
const std::uint8_t* bytes = write_batch.data();
|
||||
std::size_t remaining = batch_count * kFrameByteCount;
|
||||
while (remaining != 0U) {
|
||||
const ssize_t written = ::write(impl_->fd, bytes, remaining);
|
||||
if (written < 0) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
if ((errno == EAGAIN) || (errno == EWOULDBLOCK) || (errno == EIO)) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (impl_->stop_requested) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
continue;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (!impl_->stop_requested) {
|
||||
impl_->failure = std::make_exception_ptr(
|
||||
std::runtime_error(io_error("Cannot write tty frame to", path_)));
|
||||
}
|
||||
impl_->data_ready_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
if (written == 0) {
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (!impl_->stop_requested) {
|
||||
impl_->failure = std::make_exception_ptr(
|
||||
std::runtime_error("tty write returned 0 bytes for '" + path_ + "'"));
|
||||
}
|
||||
impl_->data_ready_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
bytes += static_cast<std::size_t>(written);
|
||||
remaining -= static_cast<std::size_t>(written);
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
impl_->stats.frames_written += static_cast<std::uint64_t>(batch_count);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,55 @@
|
||||
#pragma once
|
||||
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
struct ChannelTrace {
|
||||
std::vector<float> frequency_hz{};
|
||||
std::vector<ipc::Complex32> samples{};
|
||||
};
|
||||
|
||||
class S21CalibrationBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
void apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
std::span<ipc::Complex32> measured
|
||||
) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
class S21ReferenceBundle {
|
||||
public:
|
||||
// Load and through-calibrate the active references once at startup.
|
||||
void load(const std::string& path, const S21CalibrationBundle& calibration,
|
||||
const std::vector<ipc::ComboKey>& combos);
|
||||
void apply(ipc::SweepTraceBlock& trace) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
// Object-derived MIMO correction, exported on each pair's runtime frequency grid.
|
||||
// An empty path disables it; enabled bundles must cover every active pair.
|
||||
class S21ObjectCalibrationBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
void apply(ipc::SweepTraceBlock& trace) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> factors_by_combo_{};
|
||||
};
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
@@ -0,0 +1,255 @@
|
||||
#include "channel_bundle_support.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <iterator>
|
||||
#include <limits>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
constexpr float kFrequencyRelativeTolerance = 1e-5F;
|
||||
constexpr float kFrequencyAbsoluteTolerance = 1e-3F;
|
||||
[[nodiscard]] auto combo_to_string(const ipc::ComboKey& combo) -> std::string {
|
||||
return "input=" + std::to_string(combo.input_pos) + " output=" + std::to_string(combo.output_pos);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_binary_file(const std::string& path, const std::string& bundle_label)
|
||||
-> std::vector<std::uint8_t> {
|
||||
std::ifstream stream(path, std::ios::binary);
|
||||
if (!stream.is_open()) {
|
||||
throw std::runtime_error("Failed to open " + bundle_label + " bundle: " + path);
|
||||
}
|
||||
|
||||
return std::vector<std::uint8_t>(std::istreambuf_iterator<char>(stream), std::istreambuf_iterator<char>());
|
||||
}
|
||||
|
||||
void validate_channel_trace_layout(
|
||||
const ChannelTrace& trace,
|
||||
const std::string& trace_label,
|
||||
const ipc::ComboKey& combo
|
||||
) {
|
||||
if (trace.frequency_hz.size() != trace.samples.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/channel vector size mismatch for combo " + combo_to_string(combo)
|
||||
);
|
||||
}
|
||||
for (std::size_t i = 0; i < trace.samples.size(); ++i) {
|
||||
if (!std::isfinite(trace.frequency_hz[i]) || !std::isfinite(trace.samples[i].re)
|
||||
|| !std::isfinite(trace.samples[i].im)) {
|
||||
throw std::runtime_error(trace_label + " contains non-finite values for combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto frequency_axes_match(std::span<const float> left, std::span<const float> right) -> bool {
|
||||
if (left.size() != right.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (std::size_t index = 0; index < left.size(); ++index) {
|
||||
if (!std::isfinite(left[index]) || !std::isfinite(right[index])) {
|
||||
return false;
|
||||
}
|
||||
const auto delta = std::fabs(left[index] - right[index]);
|
||||
const auto scale = std::max(std::fabs(left[index]), std::fabs(right[index]));
|
||||
const auto tolerance = std::max(kFrequencyAbsoluteTolerance, scale * kFrequencyRelativeTolerance);
|
||||
if (delta > tolerance) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ensure_frequency_axis_match(
|
||||
std::span<const float> expected,
|
||||
std::span<const float> actual,
|
||||
const std::string& label
|
||||
) {
|
||||
if (!frequency_axes_match(expected, actual)) {
|
||||
throw std::runtime_error(label + " frequency axis mismatch");
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_channel_trace(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const std::string& bundle_label
|
||||
) -> ChannelTrace {
|
||||
if (trace.frequency_hz.empty()) {
|
||||
throw std::runtime_error(bundle_label + " trace contains no frequency samples");
|
||||
}
|
||||
|
||||
ChannelTrace channel_trace{};
|
||||
channel_trace.frequency_hz = trace.frequency_hz;
|
||||
channel_trace.samples = trace.s21;
|
||||
validate_channel_trace_layout(channel_trace, bundle_label + " trace", trace.combo);
|
||||
return channel_trace;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto load_channel_traces(
|
||||
const std::string& path,
|
||||
const std::string& bundle_label
|
||||
) -> std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> {
|
||||
const auto bytes = read_binary_file(path, bundle_label);
|
||||
if (bytes.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle is empty: " + path);
|
||||
}
|
||||
|
||||
const auto collection = ipc::deserialize_raw_collection(bytes);
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo{};
|
||||
traces_by_combo.reserve(collection.traces.size());
|
||||
for (const auto& trace : collection.traces) {
|
||||
// #47: reject duplicate combos instead of silently overwriting an earlier trace.
|
||||
const auto inserted =
|
||||
traces_by_combo.insert({trace.combo, to_channel_trace(trace, bundle_label)});
|
||||
if (!inserted.second) {
|
||||
throw std::runtime_error(
|
||||
bundle_label + " bundle contains duplicate combo " + combo_to_string(trace.combo) + ": " + path
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
if (traces_by_combo.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle does not contain traces: " + path);
|
||||
}
|
||||
return traces_by_combo;
|
||||
}
|
||||
|
||||
void validate_bundle_combos(
|
||||
const std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash>& traces_by_combo,
|
||||
const std::vector<ipc::ComboKey>& combos,
|
||||
const std::string& bundle_label
|
||||
) {
|
||||
if (traces_by_combo.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (const auto& combo : combos) {
|
||||
if (!traces_by_combo.contains(combo)) {
|
||||
throw std::runtime_error(bundle_label + " is missing combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename TValue>
|
||||
auto resolve_required(
|
||||
const std::unordered_map<ipc::ComboKey, TValue, ipc::ComboKeyHash>& values_by_combo,
|
||||
const ipc::ComboKey& combo,
|
||||
const char* value_label
|
||||
) -> const TValue& {
|
||||
const auto found = values_by_combo.find(combo);
|
||||
if (found == values_by_combo.end()) {
|
||||
throw std::runtime_error(std::string(value_label) + " is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
return found->second;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void S21CalibrationBundle::load(const std::string& path) {
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("S21 calibration bundle path must not be empty");
|
||||
}
|
||||
|
||||
traces_by_combo_ = load_channel_traces(path, "S21 calibration");
|
||||
}
|
||||
|
||||
void S21CalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(traces_by_combo_, combos, "S21 calibration bundle");
|
||||
}
|
||||
|
||||
void S21CalibrationBundle::apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
std::span<ipc::Complex32> measured
|
||||
) const {
|
||||
const auto& calibration_trace = resolve_required(traces_by_combo_, combo, "S21 calibration trace");
|
||||
ensure_frequency_axis_match(calibration_trace.frequency_hz, frequency_hz, "S21 calibration");
|
||||
if (measured.size() != calibration_trace.samples.size()) {
|
||||
throw std::runtime_error("S21 calibration vector size mismatch");
|
||||
}
|
||||
constexpr float epsilon = 1e-12F;
|
||||
for (std::size_t index = 0; index < measured.size(); ++index) {
|
||||
const auto standard = calibration_trace.samples[index];
|
||||
const float magnitude_squared = standard.re * standard.re + standard.im * standard.im;
|
||||
if (magnitude_squared > epsilon) {
|
||||
const auto value = measured[index];
|
||||
measured[index] = {
|
||||
(value.re * standard.re + value.im * standard.im) / magnitude_squared,
|
||||
(value.im * standard.re - value.re * standard.im) / magnitude_squared,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::load(
|
||||
const std::string& path,
|
||||
const S21CalibrationBundle& calibration,
|
||||
const std::vector<ipc::ComboKey>& combos
|
||||
) {
|
||||
const auto raw = load_channel_traces(path, "S21 reference");
|
||||
decltype(traces_by_combo_) calibrated;
|
||||
for (const auto& combo : combos) {
|
||||
auto trace = resolve_required(raw, combo, "S21 reference");
|
||||
calibration.apply(combo, trace.frequency_hz, trace.samples);
|
||||
validate_channel_trace_layout(trace, "Calibrated S21 reference", combo);
|
||||
calibrated.emplace(combo, std::move(trace));
|
||||
}
|
||||
traces_by_combo_ = std::move(calibrated);
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::apply(ipc::SweepTraceBlock& trace) const {
|
||||
const auto& reference = resolve_required(traces_by_combo_, trace.combo, "Calibrated S21 reference");
|
||||
ensure_frequency_axis_match(reference.frequency_hz, trace.frequency_hz, "S21 reference");
|
||||
if (trace.s21.size() != reference.samples.size()) {
|
||||
throw std::runtime_error("S21 reference point count mismatch");
|
||||
}
|
||||
for (std::size_t index = 0; index < trace.s21.size(); ++index) {
|
||||
trace.s21[index].re -= reference.samples[index].re;
|
||||
trace.s21[index].im -= reference.samples[index].im;
|
||||
}
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::load(const std::string& path) {
|
||||
if (path.empty()) {
|
||||
factors_by_combo_.clear();
|
||||
return;
|
||||
}
|
||||
factors_by_combo_ = load_channel_traces(path, "S21 object calibration");
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(factors_by_combo_, combos, "S21 object calibration bundle");
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::apply(ipc::SweepTraceBlock& trace) const {
|
||||
if (factors_by_combo_.empty()) { return; }
|
||||
const auto& factors = resolve_required(factors_by_combo_, trace.combo, "S21 object calibration");
|
||||
ensure_frequency_axis_match(factors.frequency_hz, trace.frequency_hz, "S21 object calibration");
|
||||
if (trace.s21.size() != factors.samples.size()) {
|
||||
throw std::runtime_error("S21 object calibration vector size mismatch");
|
||||
}
|
||||
for (std::size_t index = 0; index < trace.s21.size(); ++index) {
|
||||
const auto value = trace.s21[index];
|
||||
const auto factor = factors.samples[index];
|
||||
const auto corrected = std::complex<double>(value.re, value.im) * std::complex<double>(factor.re, factor.im);
|
||||
if (!std::isfinite(corrected.real()) || !std::isfinite(corrected.imag()) ||
|
||||
std::abs(corrected.real()) > std::numeric_limits<float>::max() ||
|
||||
std::abs(corrected.imag()) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("S21 object calibration result must fit finite float32");
|
||||
}
|
||||
trace.s21[index] = {.re = static_cast<float>(corrected.real()), .im = static_cast<float>(corrected.imag())};
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
-51
@@ -1,51 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "calibrator_interface.hpp"
|
||||
#include "channel_bundle_support.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
class CalibrationMaster {
|
||||
public:
|
||||
// `s21_calibrator` encapsulates the S21 calibration algorithm.
|
||||
explicit CalibrationMaster(std::unique_ptr<CalibratorInterface> s21_calibrator);
|
||||
|
||||
// Loads a serialized raw sweep bundle with one S21 calibration standard per combo.
|
||||
void load_s21_calibration_bundle(const std::string& path);
|
||||
// Loads optional one-port OSL calibration data for S11 from raw sweep bundles.
|
||||
void load_s11_calibration_bundle(
|
||||
const std::string& open_path,
|
||||
const std::string& short_path,
|
||||
const std::string& load_path
|
||||
);
|
||||
// Ensures all runtime combos are present in loaded standards.
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
// Applies both channel calibrations to one measured trace.
|
||||
[[nodiscard]] auto apply_to_trace(const ipc::SweepTraceBlock& measured_trace) const -> ipc::SweepTraceBlock;
|
||||
[[nodiscard]] auto apply_s21(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured_s21
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
[[nodiscard]] auto apply_s11(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured_s11
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
[[nodiscard]] auto has_s11_calibration() const noexcept -> bool;
|
||||
|
||||
private:
|
||||
std::unique_ptr<CalibratorInterface> s21_calibrator_;
|
||||
S21CalibrationBundle s21_calibration_bundle_{};
|
||||
S11CalibrationBundle s11_calibration_bundle_{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto make_s21_through_calibrator() -> std::unique_ptr<CalibratorInterface>;
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
-21
@@ -1,21 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
class CalibratorInterface {
|
||||
public:
|
||||
virtual ~CalibratorInterface() = default;
|
||||
|
||||
[[nodiscard]] virtual auto name() const -> std::string = 0;
|
||||
[[nodiscard]] virtual auto apply(
|
||||
const std::vector<ipc::Complex32>& measured,
|
||||
const std::vector<ipc::Complex32>& calibration
|
||||
) const -> std::vector<ipc::Complex32> = 0;
|
||||
};
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
-91
@@ -1,91 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
class CalibratorInterface;
|
||||
|
||||
struct ChannelTrace {
|
||||
std::vector<float> frequency_hz{};
|
||||
std::vector<ipc::Complex32> samples{};
|
||||
};
|
||||
|
||||
class S21CalibrationBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
|
||||
[[nodiscard]] auto is_enabled() const noexcept -> bool;
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
[[nodiscard]] auto apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured,
|
||||
const CalibratorInterface& calibrator
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
|
||||
private:
|
||||
[[nodiscard]] auto resolve(const ipc::ComboKey& combo) const -> const ChannelTrace&;
|
||||
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
class S21ReferenceBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
|
||||
[[nodiscard]] auto is_enabled() const noexcept -> bool;
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
[[nodiscard]] auto resolve(const ipc::ComboKey& combo) const -> const ChannelTrace&;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
class S11CalibrationBundle {
|
||||
public:
|
||||
struct Coefficients {
|
||||
std::vector<float> frequency_hz{};
|
||||
std::vector<ipc::Complex32> directivity{};
|
||||
std::vector<ipc::Complex32> source_match{};
|
||||
std::vector<ipc::Complex32> reflection_tracking{};
|
||||
};
|
||||
|
||||
void load(
|
||||
const std::string& open_path,
|
||||
const std::string& short_path,
|
||||
const std::string& load_path
|
||||
);
|
||||
|
||||
[[nodiscard]] auto is_enabled() const noexcept -> bool;
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
[[nodiscard]] auto apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
|
||||
private:
|
||||
[[nodiscard]] auto resolve(const ipc::ComboKey& combo) const -> const Coefficients&;
|
||||
|
||||
std::unordered_map<ipc::ComboKey, Coefficients, ipc::ComboKeyHash> coefficients_by_combo_{};
|
||||
};
|
||||
|
||||
class S11ReferenceBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
|
||||
[[nodiscard]] auto is_enabled() const noexcept -> bool;
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
[[nodiscard]] auto resolve(const ipc::ComboKey& combo) const -> const ChannelTrace&;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
@@ -1,63 +0,0 @@
|
||||
#include "calibration_master.hpp"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
CalibrationMaster::CalibrationMaster(std::unique_ptr<CalibratorInterface> s21_calibrator)
|
||||
: s21_calibrator_(std::move(s21_calibrator)) {
|
||||
if (!s21_calibrator_) {
|
||||
throw std::runtime_error("CalibrationMaster requires a non-null calibrator");
|
||||
}
|
||||
}
|
||||
|
||||
void CalibrationMaster::load_s21_calibration_bundle(const std::string& path) {
|
||||
s21_calibration_bundle_.load(path);
|
||||
}
|
||||
|
||||
void CalibrationMaster::load_s11_calibration_bundle(
|
||||
const std::string& open_path,
|
||||
const std::string& short_path,
|
||||
const std::string& load_path
|
||||
) {
|
||||
s11_calibration_bundle_.load(open_path, short_path, load_path);
|
||||
}
|
||||
|
||||
void CalibrationMaster::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
s21_calibration_bundle_.validate_combos(combos);
|
||||
s11_calibration_bundle_.validate_combos(combos);
|
||||
}
|
||||
|
||||
auto CalibrationMaster::apply_to_trace(const ipc::SweepTraceBlock& measured_trace) const -> ipc::SweepTraceBlock {
|
||||
ipc::SweepTraceBlock output{};
|
||||
output.combo = measured_trace.combo;
|
||||
output.frequency_hz = measured_trace.frequency_hz;
|
||||
output.s21 = apply_s21(measured_trace.combo, measured_trace.frequency_hz, measured_trace.s21);
|
||||
output.s11 = apply_s11(measured_trace.combo, measured_trace.frequency_hz, measured_trace.s11);
|
||||
// Calibration reshapes the samples, not when they were measured.
|
||||
output.capture_start_ns = measured_trace.capture_start_ns;
|
||||
output.capture_end_ns = measured_trace.capture_end_ns;
|
||||
return output;
|
||||
}
|
||||
|
||||
auto CalibrationMaster::apply_s21(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured_s21
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
return s21_calibration_bundle_.apply(combo, frequency_hz, measured_s21, *s21_calibrator_);
|
||||
}
|
||||
|
||||
auto CalibrationMaster::apply_s11(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured_s11
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
return s11_calibration_bundle_.apply(combo, frequency_hz, measured_s11);
|
||||
}
|
||||
|
||||
auto CalibrationMaster::has_s11_calibration() const noexcept -> bool {
|
||||
return s11_calibration_bundle_.is_enabled();
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
-448
@@ -1,448 +0,0 @@
|
||||
#include "channel_bundle_support.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <iterator>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "calibrator_interface.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
enum class RawTraceChannel {
|
||||
S11,
|
||||
S21,
|
||||
};
|
||||
|
||||
constexpr float kFrequencyRelativeTolerance = 1e-5F;
|
||||
constexpr float kFrequencyAbsoluteTolerance = 1e-3F;
|
||||
constexpr float kComplexMagnitudeSquaredEpsilon = 1e-12F;
|
||||
// #30: OSL denominator must be well conditioned relative to the magnitudes of the terms forming it.
|
||||
constexpr float kOslRelativeConditioning = 1e-6F;
|
||||
// #30: maximum fraction of degenerate (fallback) OSL points tolerated per combo before load fails.
|
||||
constexpr float kOslMaxDegenerateFraction = 0.05F;
|
||||
|
||||
[[nodiscard]] auto combo_to_string(const ipc::ComboKey& combo) -> std::string {
|
||||
return "input=" + std::to_string(combo.input_pos) + " output=" + std::to_string(combo.output_pos);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_binary_file(const std::string& path, const std::string& bundle_label)
|
||||
-> std::vector<std::uint8_t> {
|
||||
std::ifstream stream(path, std::ios::binary);
|
||||
if (!stream.is_open()) {
|
||||
throw std::runtime_error("Failed to open " + bundle_label + " bundle: " + path);
|
||||
}
|
||||
|
||||
return std::vector<std::uint8_t>(std::istreambuf_iterator<char>(stream), std::istreambuf_iterator<char>());
|
||||
}
|
||||
|
||||
void validate_raw_trace_layout(const ipc::SweepTraceBlock& trace, const std::string& trace_label) {
|
||||
if (trace.frequency_hz.size() != trace.s11.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/S11 vector size mismatch for combo " + combo_to_string(trace.combo)
|
||||
);
|
||||
}
|
||||
if (trace.frequency_hz.size() != trace.s21.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/S21 vector size mismatch for combo " + combo_to_string(trace.combo)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
void validate_channel_trace_layout(
|
||||
const ChannelTrace& trace,
|
||||
const std::string& trace_label,
|
||||
const ipc::ComboKey& combo
|
||||
) {
|
||||
if (trace.frequency_hz.size() != trace.samples.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/channel vector size mismatch for combo " + combo_to_string(combo)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto frequency_axes_match(std::span<const float> left, std::span<const float> right) -> bool {
|
||||
if (left.size() != right.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (std::size_t index = 0; index < left.size(); ++index) {
|
||||
const auto delta = std::fabs(left[index] - right[index]);
|
||||
const auto scale = std::max(std::fabs(left[index]), std::fabs(right[index]));
|
||||
const auto tolerance = std::max(kFrequencyAbsoluteTolerance, scale * kFrequencyRelativeTolerance);
|
||||
if (delta > tolerance) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ensure_frequency_axis_match(
|
||||
std::span<const float> expected,
|
||||
std::span<const float> actual,
|
||||
const std::string& label
|
||||
) {
|
||||
if (!frequency_axes_match(expected, actual)) {
|
||||
throw std::runtime_error(label + " frequency axis mismatch");
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_std_complex(const ipc::Complex32& value) -> std::complex<float> {
|
||||
return std::complex<float>(value.re, value.im);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto from_std_complex(const std::complex<float>& value) -> ipc::Complex32 {
|
||||
return ipc::Complex32{
|
||||
.re = value.real(),
|
||||
.im = value.imag(),
|
||||
};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto pick_channel_samples(const ipc::SweepTraceBlock& trace, RawTraceChannel channel)
|
||||
-> const std::vector<ipc::Complex32>& {
|
||||
if (channel == RawTraceChannel::S11) {
|
||||
return trace.s11;
|
||||
}
|
||||
return trace.s21;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_channel_trace(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
RawTraceChannel channel,
|
||||
const std::string& bundle_label
|
||||
) -> ChannelTrace {
|
||||
validate_raw_trace_layout(trace, bundle_label + " trace");
|
||||
|
||||
ChannelTrace channel_trace{};
|
||||
channel_trace.frequency_hz = trace.frequency_hz;
|
||||
channel_trace.samples = pick_channel_samples(trace, channel);
|
||||
validate_channel_trace_layout(channel_trace, bundle_label + " trace", trace.combo);
|
||||
return channel_trace;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto load_channel_traces(
|
||||
const std::string& path,
|
||||
const std::string& bundle_label,
|
||||
RawTraceChannel channel
|
||||
) -> std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> {
|
||||
const auto bytes = read_binary_file(path, bundle_label);
|
||||
if (bytes.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle is empty: " + path);
|
||||
}
|
||||
|
||||
const auto collection = ipc::deserialize_raw_collection(bytes);
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo{};
|
||||
traces_by_combo.reserve(collection.traces.size());
|
||||
for (const auto& trace : collection.traces) {
|
||||
// #47: reject duplicate combos instead of silently overwriting an earlier trace.
|
||||
const auto inserted =
|
||||
traces_by_combo.insert({trace.combo, to_channel_trace(trace, channel, bundle_label)});
|
||||
if (!inserted.second) {
|
||||
throw std::runtime_error(
|
||||
bundle_label + " bundle contains duplicate combo " + combo_to_string(trace.combo) + ": " + path
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
if (traces_by_combo.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle does not contain traces: " + path);
|
||||
}
|
||||
return traces_by_combo;
|
||||
}
|
||||
|
||||
void validate_bundle_combos(
|
||||
const std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash>& traces_by_combo,
|
||||
const std::vector<ipc::ComboKey>& combos,
|
||||
const std::string& bundle_label
|
||||
) {
|
||||
if (traces_by_combo.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (const auto& combo : combos) {
|
||||
if (!traces_by_combo.contains(combo)) {
|
||||
throw std::runtime_error(bundle_label + " is missing combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename TValue>
|
||||
auto resolve_required(
|
||||
const std::unordered_map<ipc::ComboKey, TValue, ipc::ComboKeyHash>& values_by_combo,
|
||||
const ipc::ComboKey& combo,
|
||||
const std::string& value_label
|
||||
) -> const TValue& {
|
||||
const auto found = values_by_combo.find(combo);
|
||||
if (found == values_by_combo.end()) {
|
||||
throw std::runtime_error(value_label + " is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
return found->second;
|
||||
}
|
||||
|
||||
void ensure_combo_coverage(
|
||||
const std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash>& expected,
|
||||
const std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash>& actual,
|
||||
const std::string& expected_label,
|
||||
const std::string& actual_label
|
||||
) {
|
||||
for (const auto& entry : expected) {
|
||||
const auto& combo = entry.first;
|
||||
if (!actual.contains(combo)) {
|
||||
throw std::runtime_error(
|
||||
actual_label + " is missing combo " + combo_to_string(combo) +
|
||||
" required by " + expected_label
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto solve_osl_coefficients(
|
||||
const ipc::ComboKey& combo,
|
||||
const ChannelTrace& open_trace,
|
||||
const ChannelTrace& short_trace,
|
||||
const ChannelTrace& load_trace
|
||||
) -> S11CalibrationBundle::Coefficients {
|
||||
validate_channel_trace_layout(open_trace, "S11 open calibration", combo);
|
||||
validate_channel_trace_layout(short_trace, "S11 short calibration", combo);
|
||||
validate_channel_trace_layout(load_trace, "S11 load calibration", combo);
|
||||
ensure_frequency_axis_match(open_trace.frequency_hz, short_trace.frequency_hz, "S11 short calibration");
|
||||
ensure_frequency_axis_match(open_trace.frequency_hz, load_trace.frequency_hz, "S11 load calibration");
|
||||
|
||||
S11CalibrationBundle::Coefficients coefficients{};
|
||||
coefficients.frequency_hz = open_trace.frequency_hz;
|
||||
coefficients.directivity.resize(open_trace.frequency_hz.size());
|
||||
coefficients.source_match.resize(open_trace.frequency_hz.size());
|
||||
coefficients.reflection_tracking.resize(open_trace.frequency_hz.size());
|
||||
|
||||
// #30: track degenerate points so silently-substituted fallback coefficients cannot pass unnoticed.
|
||||
std::size_t degenerate_points = 0;
|
||||
for (std::size_t index = 0; index < open_trace.frequency_hz.size(); ++index) {
|
||||
const auto load = to_std_complex(load_trace.samples[index]);
|
||||
const auto open_delta = to_std_complex(open_trace.samples[index]) - load;
|
||||
const auto short_delta = to_std_complex(short_trace.samples[index]) - load;
|
||||
const auto denominator = open_delta - short_delta;
|
||||
|
||||
// #30: require the denominator to be well conditioned both absolutely and relative to the
|
||||
// magnitudes of the open/short deltas it is formed from, so near-cancellations are rejected.
|
||||
const auto denominator_norm = std::norm(denominator);
|
||||
const auto relative_scale = std::max(std::norm(open_delta), std::norm(short_delta));
|
||||
const auto relative_threshold = relative_scale * (kOslRelativeConditioning * kOslRelativeConditioning);
|
||||
|
||||
std::complex<float> source_match = std::complex<float>(0.0F, 0.0F);
|
||||
std::complex<float> reflection_tracking = std::complex<float>(1.0F, 0.0F);
|
||||
if (denominator_norm > kComplexMagnitudeSquaredEpsilon && denominator_norm > relative_threshold) {
|
||||
source_match = (open_delta + short_delta) / denominator;
|
||||
reflection_tracking = open_delta * (std::complex<float>(1.0F, 0.0F) - source_match);
|
||||
} else {
|
||||
// #30: degenerate point — log the offending combo/frequency and fall back to a pass-through.
|
||||
++degenerate_points;
|
||||
std::cerr << "S11 calibration: degenerate OSL point for combo " << combo_to_string(combo)
|
||||
<< " at " << open_trace.frequency_hz[index] << " Hz (ill-conditioned denominator)\n";
|
||||
}
|
||||
|
||||
coefficients.directivity[index] = load_trace.samples[index];
|
||||
coefficients.source_match[index] = from_std_complex(source_match);
|
||||
coefficients.reflection_tracking[index] = from_std_complex(reflection_tracking);
|
||||
}
|
||||
|
||||
// #30: refuse to load a calibration whose per-combo fallback fraction exceeds the tolerated threshold.
|
||||
if (!open_trace.frequency_hz.empty()) {
|
||||
const auto degenerate_fraction =
|
||||
static_cast<float>(degenerate_points) / static_cast<float>(open_trace.frequency_hz.size());
|
||||
if (degenerate_fraction > kOslMaxDegenerateFraction) {
|
||||
throw std::runtime_error(
|
||||
"S11 calibration for combo " + combo_to_string(combo) + " has " +
|
||||
std::to_string(degenerate_points) + " of " +
|
||||
std::to_string(open_trace.frequency_hz.size()) +
|
||||
" degenerate OSL points (fraction exceeds tolerance)"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
return coefficients;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void S21CalibrationBundle::load(const std::string& path) {
|
||||
traces_by_combo_.clear();
|
||||
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("S21 calibration bundle path must not be empty");
|
||||
}
|
||||
|
||||
traces_by_combo_ = load_channel_traces(path, "S21 calibration", RawTraceChannel::S21);
|
||||
}
|
||||
|
||||
auto S21CalibrationBundle::is_enabled() const noexcept -> bool {
|
||||
return !traces_by_combo_.empty();
|
||||
}
|
||||
|
||||
void S21CalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(traces_by_combo_, combos, "S21 calibration bundle");
|
||||
}
|
||||
|
||||
auto S21CalibrationBundle::apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured,
|
||||
const CalibratorInterface& calibrator
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
const auto& calibration_trace = resolve(combo);
|
||||
ensure_frequency_axis_match(calibration_trace.frequency_hz, frequency_hz, "S21 calibration");
|
||||
if (measured.size() != calibration_trace.samples.size()) {
|
||||
throw std::runtime_error("S21 calibration vector size mismatch");
|
||||
}
|
||||
return calibrator.apply(measured, calibration_trace.samples);
|
||||
}
|
||||
|
||||
auto S21CalibrationBundle::resolve(const ipc::ComboKey& combo) const -> const ChannelTrace& {
|
||||
return resolve_required(traces_by_combo_, combo, "S21 calibration trace");
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::load(const std::string& path) {
|
||||
traces_by_combo_.clear();
|
||||
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("S21 reference bundle path must not be empty");
|
||||
}
|
||||
|
||||
traces_by_combo_ = load_channel_traces(path, "S21 reference", RawTraceChannel::S21);
|
||||
}
|
||||
|
||||
auto S21ReferenceBundle::is_enabled() const noexcept -> bool {
|
||||
return !traces_by_combo_.empty();
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(traces_by_combo_, combos, "S21 reference bundle");
|
||||
}
|
||||
|
||||
auto S21ReferenceBundle::resolve(const ipc::ComboKey& combo) const -> const ChannelTrace& {
|
||||
return resolve_required(traces_by_combo_, combo, "S21 reference trace");
|
||||
}
|
||||
|
||||
void S11CalibrationBundle::load(
|
||||
const std::string& open_path,
|
||||
const std::string& short_path,
|
||||
const std::string& load_path
|
||||
) {
|
||||
coefficients_by_combo_.clear();
|
||||
|
||||
if (open_path.empty() && short_path.empty() && load_path.empty()) {
|
||||
// #38: S11 calibration disabled because no OSL paths were provided; surface it instead of silently skipping.
|
||||
std::cerr << "S11 calibration: no open/short/load bundle paths provided; reflection correction disabled\n";
|
||||
return;
|
||||
}
|
||||
if (open_path.empty() || short_path.empty() || load_path.empty()) {
|
||||
throw std::runtime_error("S11 calibration requires open, short, and load raw bundle paths");
|
||||
}
|
||||
|
||||
const auto open_traces = load_channel_traces(open_path, "S11 open calibration", RawTraceChannel::S11);
|
||||
const auto short_traces = load_channel_traces(short_path, "S11 short calibration", RawTraceChannel::S11);
|
||||
const auto load_traces = load_channel_traces(load_path, "S11 load calibration", RawTraceChannel::S11);
|
||||
|
||||
ensure_combo_coverage(open_traces, short_traces, "S11 open calibration", "S11 short calibration");
|
||||
ensure_combo_coverage(open_traces, load_traces, "S11 open calibration", "S11 load calibration");
|
||||
ensure_combo_coverage(short_traces, open_traces, "S11 short calibration", "S11 open calibration");
|
||||
ensure_combo_coverage(load_traces, open_traces, "S11 load calibration", "S11 open calibration");
|
||||
|
||||
coefficients_by_combo_.reserve(open_traces.size());
|
||||
for (const auto& [combo, open_trace] : open_traces) {
|
||||
coefficients_by_combo_.insert_or_assign(
|
||||
combo,
|
||||
solve_osl_coefficients(combo, open_trace, short_traces.at(combo), load_traces.at(combo))
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
auto S11CalibrationBundle::is_enabled() const noexcept -> bool {
|
||||
return !coefficients_by_combo_.empty();
|
||||
}
|
||||
|
||||
void S11CalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
if (!is_enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (const auto& combo : combos) {
|
||||
if (!coefficients_by_combo_.contains(combo)) {
|
||||
throw std::runtime_error("S11 calibration data is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto S11CalibrationBundle::apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& measured
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
if (!is_enabled()) {
|
||||
return measured;
|
||||
}
|
||||
if (frequency_hz.size() != measured.size()) {
|
||||
throw std::runtime_error("S11 calibration input frequency/sample size mismatch");
|
||||
}
|
||||
|
||||
const auto& coefficients = resolve(combo);
|
||||
ensure_frequency_axis_match(coefficients.frequency_hz, frequency_hz, "S11 calibration");
|
||||
if (measured.size() != coefficients.directivity.size()) {
|
||||
throw std::runtime_error("S11 calibration vector size mismatch");
|
||||
}
|
||||
|
||||
std::vector<ipc::Complex32> corrected{};
|
||||
corrected.reserve(measured.size());
|
||||
for (std::size_t index = 0; index < measured.size(); ++index) {
|
||||
const auto numerator = to_std_complex(measured[index]) - to_std_complex(coefficients.directivity[index]);
|
||||
const auto denominator =
|
||||
to_std_complex(coefficients.reflection_tracking[index]) +
|
||||
(to_std_complex(coefficients.source_match[index]) * numerator);
|
||||
|
||||
if (std::norm(denominator) > kComplexMagnitudeSquaredEpsilon) {
|
||||
corrected.push_back(from_std_complex(numerator / denominator));
|
||||
} else {
|
||||
corrected.push_back(from_std_complex(numerator));
|
||||
}
|
||||
}
|
||||
return corrected;
|
||||
}
|
||||
|
||||
auto S11CalibrationBundle::resolve(const ipc::ComboKey& combo) const -> const Coefficients& {
|
||||
return resolve_required(coefficients_by_combo_, combo, "S11 calibration coefficients");
|
||||
}
|
||||
|
||||
void S11ReferenceBundle::load(const std::string& path) {
|
||||
traces_by_combo_.clear();
|
||||
|
||||
if (path.empty()) {
|
||||
// #38: S11 reference disabled because no path was provided; surface it instead of silently skipping.
|
||||
std::cerr << "S11 reference: no bundle path provided; reference normalization disabled\n";
|
||||
return;
|
||||
}
|
||||
|
||||
traces_by_combo_ = load_channel_traces(path, "S11 reference", RawTraceChannel::S11);
|
||||
}
|
||||
|
||||
auto S11ReferenceBundle::is_enabled() const noexcept -> bool {
|
||||
return !traces_by_combo_.empty();
|
||||
}
|
||||
|
||||
void S11ReferenceBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(traces_by_combo_, combos, "S11 reference bundle");
|
||||
}
|
||||
|
||||
auto S11ReferenceBundle::resolve(const ipc::ComboKey& combo) const -> const ChannelTrace& {
|
||||
return resolve_required(traces_by_combo_, combo, "S11 reference trace");
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
@@ -1,76 +0,0 @@
|
||||
#include "calibration_master.hpp"
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
class ThroughCalibrator final : public CalibratorInterface {
|
||||
public:
|
||||
[[nodiscard]] auto name() const -> std::string override {
|
||||
return "through";
|
||||
}
|
||||
|
||||
[[nodiscard]] auto apply(
|
||||
const std::vector<ipc::Complex32>& measured,
|
||||
const std::vector<ipc::Complex32>& calibration
|
||||
) const -> std::vector<ipc::Complex32> override {
|
||||
if (measured.size() != calibration.size()) {
|
||||
throw std::runtime_error("Through calibration vector size mismatch");
|
||||
}
|
||||
|
||||
static_assert(sizeof(ipc::Complex32) == sizeof(float) * 2U, "Complex32 layout must be two contiguous floats");
|
||||
|
||||
constexpr float epsilon = 1e-12F;
|
||||
using InterleavedComplexView = Eigen::Matrix<float, Eigen::Dynamic, 2, Eigen::RowMajor>;
|
||||
|
||||
const auto point_count = static_cast<Eigen::Index>(measured.size());
|
||||
Eigen::Map<const InterleavedComplexView> measured_view(
|
||||
reinterpret_cast<const float*>(measured.data()),
|
||||
point_count,
|
||||
2
|
||||
);
|
||||
Eigen::Map<const InterleavedComplexView> calibration_view(
|
||||
reinterpret_cast<const float*>(calibration.data()),
|
||||
point_count,
|
||||
2
|
||||
);
|
||||
|
||||
const auto measured_re = measured_view.col(0).array();
|
||||
const auto measured_im = measured_view.col(1).array();
|
||||
const auto calibration_re = calibration_view.col(0).array();
|
||||
const auto calibration_im = calibration_view.col(1).array();
|
||||
|
||||
const Eigen::ArrayXf magnitude_squared = calibration_re.square() + calibration_im.square();
|
||||
const auto stable_division_mask = (magnitude_squared > epsilon);
|
||||
|
||||
const Eigen::ArrayXf corrected_re = stable_division_mask.select(
|
||||
((measured_re * calibration_re) + (measured_im * calibration_im)) / magnitude_squared,
|
||||
measured_re
|
||||
);
|
||||
const Eigen::ArrayXf corrected_im = stable_division_mask.select(
|
||||
((measured_im * calibration_re) - (measured_re * calibration_im)) / magnitude_squared,
|
||||
measured_im
|
||||
);
|
||||
|
||||
std::vector<ipc::Complex32> corrected(measured.size());
|
||||
Eigen::Map<InterleavedComplexView> corrected_view(reinterpret_cast<float*>(corrected.data()), point_count, 2);
|
||||
corrected_view.col(0) = corrected_re.matrix();
|
||||
corrected_view.col(1) = corrected_im.matrix();
|
||||
|
||||
return corrected;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
auto make_s21_through_calibrator() -> std::unique_ptr<CalibratorInterface> {
|
||||
return std::make_unique<ThroughCalibrator>();
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
+14
-13
@@ -2,21 +2,27 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include <vector>
|
||||
|
||||
#include "calibration_master.hpp"
|
||||
#include "reference_master.hpp"
|
||||
#include "channel_bundle_support.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
// Per-point notch multipliers: 0 inside every band, 1 outside; with the cosine
|
||||
// taper a raised-cosine shoulder of taper_width_hz falls from 1 to 0 towards
|
||||
// each band edge.
|
||||
[[nodiscard]] auto build_notch_mask(
|
||||
std::span<const float> frequency_hz,
|
||||
const config::PreprocessNotchConfig& notch
|
||||
) -> std::vector<float>;
|
||||
|
||||
class DataPreprocessor {
|
||||
public:
|
||||
DataPreprocessor(
|
||||
const config::RunConfig& config,
|
||||
CalibrationMaster& calibration_master,
|
||||
ReferenceMaster& reference_master,
|
||||
ipc::ShmRing& raw_ring,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing* preprocessed_tap_ring = nullptr
|
||||
@@ -26,19 +32,14 @@ class DataPreprocessor {
|
||||
void run(const std::atomic<bool>& stop_requested);
|
||||
|
||||
private:
|
||||
void validate_startup_prerequisites() const;
|
||||
[[nodiscard]] auto try_pop_raw_collection(
|
||||
std::vector<std::uint8_t>& serialized_raw,
|
||||
ipc::RawSweepCollection* out_collection
|
||||
) -> bool;
|
||||
void publish_preprocessed_collection(const ipc::PreprocessedCollection& collection);
|
||||
|
||||
[[nodiscard]] auto preprocess_collection(const ipc::RawSweepCollection& raw_collection) const
|
||||
-> ipc::PreprocessedCollection;
|
||||
void preprocess_collection(ipc::RawSweepCollection& collection) const;
|
||||
|
||||
const config::RunConfig& config_;
|
||||
CalibrationMaster& calibration_master_;
|
||||
ReferenceMaster& reference_master_;
|
||||
S21CalibrationBundle calibration_;
|
||||
S21ReferenceBundle reference_;
|
||||
S21ObjectCalibrationBundle object_calibration_;
|
||||
ipc::ShmRing& raw_ring_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing* preprocessed_tap_ring_ = nullptr;
|
||||
|
||||
@@ -1,26 +1,25 @@
|
||||
#include "data_preprocessor.hpp"
|
||||
#include "runtime_wait.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <numbers>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string_view>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
[[nodiscard]] auto should_apply_notch(const config::PreprocessNotchConfig& notch) -> bool {
|
||||
return notch.enabled && !notch.bands_hz.empty();
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_notch_mask(
|
||||
} // namespace
|
||||
|
||||
auto build_notch_mask(
|
||||
std::span<const float> frequency_hz,
|
||||
const config::PreprocessNotchConfig& notch
|
||||
) -> std::vector<float> {
|
||||
@@ -51,15 +50,16 @@ constexpr double kPi = 3.14159265358979323846;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Raised-cosine shoulders: 1 at the outer edge, 0 where the band starts.
|
||||
if (frequency >= trans_low_hz && frequency < low_hz) {
|
||||
const double arg = kPi * (frequency - trans_low_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 - std::cos(arg)));
|
||||
const double arg = std::numbers::pi * (frequency - trans_low_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 + std::cos(arg)));
|
||||
continue;
|
||||
}
|
||||
|
||||
if (frequency > high_hz && frequency <= trans_high_hz) {
|
||||
const double arg = kPi * (frequency - high_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 + std::cos(arg)));
|
||||
const double arg = std::numbers::pi * (frequency - high_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 - std::cos(arg)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -67,6 +67,8 @@ constexpr double kPi = 3.14159265358979323846;
|
||||
return mask;
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
void apply_notch_mask(std::vector<ipc::Complex32>& samples, std::span<const float> mask) {
|
||||
const std::size_t point_count = std::min(samples.size(), mask.size());
|
||||
for (std::size_t index = 0U; index < point_count; ++index) {
|
||||
@@ -75,127 +77,72 @@ void apply_notch_mask(std::vector<ipc::Complex32>& samples, std::span<const floa
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto apply_notch_filter(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const config::PreprocessNotchConfig& notch
|
||||
) -> ipc::SweepTraceBlock {
|
||||
if (!should_apply_notch(notch)) {
|
||||
return trace;
|
||||
}
|
||||
|
||||
ipc::SweepTraceBlock filtered = trace;
|
||||
const auto mask = build_notch_mask(filtered.frequency_hz, notch);
|
||||
apply_notch_mask(filtered.s21, mask);
|
||||
apply_notch_mask(filtered.s11, mask);
|
||||
return filtered;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DataPreprocessor::DataPreprocessor(
|
||||
const config::RunConfig& config,
|
||||
CalibrationMaster& calibration_master,
|
||||
ReferenceMaster& reference_master,
|
||||
ipc::ShmRing& raw_ring,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing* preprocessed_tap_ring
|
||||
)
|
||||
: config_(config),
|
||||
calibration_master_(calibration_master),
|
||||
reference_master_(reference_master),
|
||||
raw_ring_(raw_ring),
|
||||
preprocessed_ring_(preprocessed_ring),
|
||||
preprocessed_tap_ring_(preprocessed_tap_ring) {}
|
||||
|
||||
void DataPreprocessor::validate_startup_prerequisites() const {
|
||||
calibration_master_.validate_combos(config_.run_combos);
|
||||
reference_master_.validate_combos(config_.run_combos);
|
||||
}
|
||||
|
||||
auto DataPreprocessor::try_pop_raw_collection(
|
||||
std::vector<std::uint8_t>& serialized_raw,
|
||||
ipc::RawSweepCollection* out_collection
|
||||
) -> bool {
|
||||
if (!raw_ring_.pop(serialized_raw)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.runtime.idle_sleep_ms));
|
||||
return false;
|
||||
}
|
||||
|
||||
*out_collection = ipc::deserialize_raw_collection(serialized_raw);
|
||||
return true;
|
||||
preprocessed_tap_ring_(preprocessed_tap_ring) {
|
||||
calibration_.load(config_.preprocess.s21.calibration.bundle_path);
|
||||
calibration_.validate_combos(config_.run_combos);
|
||||
reference_.load(config_.preprocess.s21.reference.bundle_path, calibration_, config_.run_combos);
|
||||
object_calibration_.load(config_.preprocess.s21.object_calibration.bundle_path);
|
||||
object_calibration_.validate_combos(config_.run_combos);
|
||||
}
|
||||
|
||||
void DataPreprocessor::publish_preprocessed_collection(const ipc::PreprocessedCollection& collection) {
|
||||
const auto serialized_preprocessed = ipc::serialize_preprocessed_collection(collection);
|
||||
if (!preprocessed_ring_.push(serialized_preprocessed)) {
|
||||
throw std::runtime_error("Preprocessed ring slot is too small for serialized collection");
|
||||
throw runtime::FatalPipelineError("Preprocessed ring slot is too small for serialized collection");
|
||||
}
|
||||
if (preprocessed_tap_ring_ != nullptr && !preprocessed_tap_ring_->push(serialized_preprocessed)) {
|
||||
throw std::runtime_error("Preprocessed tap ring slot is too small for serialized collection");
|
||||
throw runtime::FatalPipelineError("Preprocessed tap ring slot is too small for serialized collection");
|
||||
}
|
||||
}
|
||||
|
||||
void DataPreprocessor::run(const std::atomic<bool>& stop_requested) {
|
||||
validate_startup_prerequisites();
|
||||
|
||||
std::vector<std::uint8_t> serialized_raw{};
|
||||
ipc::RawSweepCollection raw_collection{};
|
||||
std::uint64_t error_count = 0;
|
||||
std::uint64_t consecutive_errors = 0;
|
||||
runtime::CollectionErrors errors;
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
try {
|
||||
if (!try_pop_raw_collection(serialized_raw, &raw_collection)) {
|
||||
if (!raw_ring_.pop(serialized_raw)) {
|
||||
(void)config::wait_or_stop(config_.runtime.idle_sleep_ms, stop_requested);
|
||||
continue;
|
||||
}
|
||||
const auto preprocessed_collection = preprocess_collection(raw_collection);
|
||||
publish_preprocessed_collection(preprocessed_collection);
|
||||
consecutive_errors = 0; // made progress; reset the escalation counter
|
||||
auto raw_collection = ipc::deserialize_raw_collection(serialized_raw);
|
||||
preprocess_collection(raw_collection);
|
||||
publish_preprocessed_collection(raw_collection);
|
||||
errors.progress();
|
||||
} catch (const runtime::FatalPipelineError&) {
|
||||
throw;
|
||||
} catch (const std::exception& exc) {
|
||||
// A ring-slot-too-small failure is a permanent config error (the slot
|
||||
// cannot hold a serialized collection): swallowing it would silently
|
||||
// drop every collection forever, so fail fast and let the operator fix it.
|
||||
if (std::string(exc.what()).find("slot is too small") != std::string::npos) {
|
||||
throw;
|
||||
}
|
||||
// A single malformed or transiently-bad collection must not kill the
|
||||
// long-running preprocessor: drop it and keep serving the next sweep.
|
||||
++error_count;
|
||||
++consecutive_errors;
|
||||
// Escalate a PERSISTENT failure loudly so a deterministic config error
|
||||
// (which drops every collection forever) is not hidden by the throttle.
|
||||
if (consecutive_errors % 500 == 0) {
|
||||
std::cerr << "data_preprocessor: " << consecutive_errors
|
||||
<< " consecutive failures — likely a permanent configuration error: "
|
||||
<< exc.what() << '\n';
|
||||
} else if (error_count % 100 == 1) {
|
||||
std::cerr << "data_preprocessor: dropped collection after error (count="
|
||||
<< error_count << "): " << exc.what() << '\n';
|
||||
}
|
||||
// Back-off floored at 1ms (the empty-ring sleep is never reached while a
|
||||
// persistent error keeps the ring non-empty) so the catch cannot busy-spin
|
||||
// a CPU core even when idle_sleep_ms is configured to 0.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(std::max<int>(1, config_.runtime.idle_sleep_ms)));
|
||||
errors.report("data_preprocessor", exc);
|
||||
(void)config::wait_or_stop(std::max(1U, config_.runtime.idle_sleep_ms), stop_requested);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto DataPreprocessor::preprocess_collection(const ipc::RawSweepCollection& raw_collection) const
|
||||
-> ipc::PreprocessedCollection {
|
||||
ipc::PreprocessedCollection preprocessed{};
|
||||
preprocessed.collection_id = raw_collection.collection_id;
|
||||
preprocessed.monotonic_ns = ipc::current_monotonic_ns();
|
||||
preprocessed.capture_start_ns = raw_collection.capture_start_ns;
|
||||
preprocessed.capture_end_ns = raw_collection.capture_end_ns;
|
||||
preprocessed.traces.reserve(raw_collection.traces.size());
|
||||
|
||||
for (const auto& raw_trace : raw_collection.traces) {
|
||||
// Pipeline order is fixed: calibration first, then reference subtraction.
|
||||
const auto calibrated = calibration_master_.apply_to_trace(raw_trace);
|
||||
const auto referenced = reference_master_.apply_to_trace(calibrated);
|
||||
preprocessed.traces.push_back(apply_notch_filter(referenced, config_.preprocess.notch));
|
||||
void DataPreprocessor::preprocess_collection(ipc::RawSweepCollection& collection) const {
|
||||
if (!collection.capture_start_ns || collection.capture_end_ns < collection.capture_start_ns) {
|
||||
throw std::runtime_error("Raw collection requires a valid acquisition window");
|
||||
}
|
||||
|
||||
return preprocessed;
|
||||
for (auto& trace : collection.traces) {
|
||||
calibration_.apply(trace.combo, trace.frequency_hz, trace.s21);
|
||||
reference_.apply(trace);
|
||||
if (should_apply_notch(config_.preprocess.notch)) {
|
||||
const auto mask = build_notch_mask(trace.frequency_hz, config_.preprocess.notch);
|
||||
apply_notch_mask(trace.s21, mask);
|
||||
}
|
||||
// Object-derived corrections act on the completed preprocessing output.
|
||||
object_calibration_.apply(trace);
|
||||
}
|
||||
collection.monotonic_ns = ipc::current_monotonic_ns();
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
|
||||
@@ -1,49 +1,14 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <optional>
|
||||
|
||||
#include "calibration_master.hpp"
|
||||
#include "data_preprocessor.hpp"
|
||||
#include "reference_master.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kDefaultConfigPath = "run_config.json";
|
||||
|
||||
std::atomic<bool> g_stop_requested{false};
|
||||
|
||||
void signal_handler(int /*signal*/) {
|
||||
g_stop_requested.store(true, std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
void install_signal_handlers() {
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_config_path(int argc, char** argv) -> std::string {
|
||||
std::string config_path = kDefaultConfigPath;
|
||||
for (int index = 1; index < argc; ++index) {
|
||||
const std::string arg = argv[index];
|
||||
if (arg == "--config" && (index + 1) < argc) {
|
||||
config_path = argv[++index];
|
||||
}
|
||||
}
|
||||
return config_path;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
install_signal_handlers();
|
||||
|
||||
try {
|
||||
const auto config_path = read_config_path(argc, argv);
|
||||
const auto config = radar::config::load_run_config(config_path);
|
||||
const auto config = radar::runtime::initialize(argc, argv);
|
||||
|
||||
auto raw_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.raw.name,
|
||||
@@ -55,37 +20,22 @@ int main(int argc, char** argv) {
|
||||
config.rings.preprocessed.capacity,
|
||||
config.rings.preprocessed.slot_size_bytes
|
||||
);
|
||||
auto preprocessed_tap_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.preprocessed_tap.name,
|
||||
config.rings.preprocessed_tap.capacity,
|
||||
config.rings.preprocessed_tap.slot_size_bytes
|
||||
);
|
||||
|
||||
// Load preprocessing assets once before entering run loop.
|
||||
radar::preprocessing::CalibrationMaster calibration_master(
|
||||
radar::preprocessing::make_s21_through_calibrator()
|
||||
);
|
||||
calibration_master.load_s21_calibration_bundle(config.preprocess.s21.calibration.bundle_path);
|
||||
calibration_master.load_s11_calibration_bundle(
|
||||
config.preprocess.s11.calibration.open.bundle_path,
|
||||
config.preprocess.s11.calibration.short_standard.bundle_path,
|
||||
config.preprocess.s11.calibration.load.bundle_path
|
||||
);
|
||||
|
||||
radar::preprocessing::ReferenceMaster reference_master;
|
||||
reference_master.load_s21_reference_bundle(config.preprocess.s21.reference.bundle_path);
|
||||
reference_master.load_s11_reference_bundle(config.preprocess.s11.reference.bundle_path);
|
||||
reference_master.prepare_calibrated(calibration_master, config.run_combos);
|
||||
std::optional<radar::ipc::ShmRing> preprocessed_tap_ring;
|
||||
if (!config.rings.preprocessed_tap.name.empty()) {
|
||||
preprocessed_tap_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.preprocessed_tap.name,
|
||||
config.rings.preprocessed_tap.capacity,
|
||||
config.rings.preprocessed_tap.slot_size_bytes
|
||||
);
|
||||
}
|
||||
|
||||
radar::preprocessing::DataPreprocessor preprocessor(
|
||||
config,
|
||||
calibration_master,
|
||||
reference_master,
|
||||
raw_ring,
|
||||
preprocessed_ring,
|
||||
&preprocessed_tap_ring
|
||||
preprocessed_tap_ring ? &*preprocessed_tap_ring : nullptr
|
||||
);
|
||||
preprocessor.run(g_stop_requested);
|
||||
preprocessor.run(radar::runtime::stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_preprocessor error: " << exception.what() << '\n';
|
||||
|
||||
@@ -1,46 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "channel_bundle_support.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
class CalibrationMaster;
|
||||
|
||||
class ReferenceMaster {
|
||||
public:
|
||||
// Loads a serialized raw sweep bundle with one S21 reference trace per combo.
|
||||
void load_s21_reference_bundle(const std::string& path);
|
||||
// Loads optional raw S11 reference data.
|
||||
void load_s11_reference_bundle(const std::string& path);
|
||||
// Builds calibrated references in memory using currently loaded raw references.
|
||||
// Must be called after load_s21_reference_bundle() and after calibration standards are loaded.
|
||||
void prepare_calibrated(const CalibrationMaster& calibration_master, const std::vector<ipc::ComboKey>& combos);
|
||||
// Ensures all runtime combos are present in loaded references.
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
// Subtracts both channel references from one calibrated trace.
|
||||
[[nodiscard]] auto apply_to_trace(const ipc::SweepTraceBlock& calibrated_trace) const -> ipc::SweepTraceBlock;
|
||||
[[nodiscard]] auto apply_s21(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& calibrated_s21
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
[[nodiscard]] auto apply_s11(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& calibrated_s11
|
||||
) const -> std::vector<ipc::Complex32>;
|
||||
|
||||
private:
|
||||
S21ReferenceBundle raw_s21_reference_bundle_{};
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> calibrated_s21_references_by_combo_{};
|
||||
S11ReferenceBundle raw_s11_reference_bundle_{};
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> calibrated_s11_references_by_combo_{};
|
||||
};
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
@@ -1,213 +0,0 @@
|
||||
#include "reference_master.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "calibration_master.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
constexpr float kFrequencyRelativeTolerance = 1e-5F;
|
||||
constexpr float kFrequencyAbsoluteTolerance = 1e-3F;
|
||||
|
||||
[[nodiscard]] auto combo_to_string(const ipc::ComboKey& combo) -> std::string {
|
||||
return "input=" + std::to_string(combo.input_pos) + " output=" + std::to_string(combo.output_pos);
|
||||
}
|
||||
|
||||
void validate_channel_trace_layout(
|
||||
const ChannelTrace& trace,
|
||||
const std::string& trace_label,
|
||||
const ipc::ComboKey& combo
|
||||
) {
|
||||
if (trace.frequency_hz.size() != trace.samples.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/channel vector size mismatch for combo " + combo_to_string(combo)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto frequency_axes_match(std::span<const float> left, std::span<const float> right) -> bool {
|
||||
if (left.size() != right.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (std::size_t index = 0; index < left.size(); ++index) {
|
||||
const auto delta = std::fabs(left[index] - right[index]);
|
||||
const auto scale = std::max(std::fabs(left[index]), std::fabs(right[index]));
|
||||
const auto tolerance = std::max(kFrequencyAbsoluteTolerance, scale * kFrequencyRelativeTolerance);
|
||||
if (delta > tolerance) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void ReferenceMaster::load_s21_reference_bundle(const std::string& path) {
|
||||
raw_s21_reference_bundle_.load(path);
|
||||
calibrated_s21_references_by_combo_.clear();
|
||||
}
|
||||
|
||||
void ReferenceMaster::load_s11_reference_bundle(const std::string& path) {
|
||||
raw_s11_reference_bundle_.load(path);
|
||||
calibrated_s11_references_by_combo_.clear();
|
||||
}
|
||||
|
||||
void ReferenceMaster::prepare_calibrated(
|
||||
const CalibrationMaster& calibration_master,
|
||||
const std::vector<ipc::ComboKey>& combos
|
||||
) {
|
||||
if (!raw_s21_reference_bundle_.is_enabled()) {
|
||||
throw std::runtime_error("Reference bundle must be loaded before prepare_calibrated()");
|
||||
}
|
||||
|
||||
calibrated_s21_references_by_combo_.clear();
|
||||
calibrated_s21_references_by_combo_.reserve(combos.size());
|
||||
for (const auto& combo : combos) {
|
||||
const auto& raw_reference = raw_s21_reference_bundle_.resolve(combo);
|
||||
ChannelTrace calibrated{};
|
||||
calibrated.frequency_hz = raw_reference.frequency_hz;
|
||||
calibrated.samples = calibration_master.apply_s21(combo, raw_reference.frequency_hz, raw_reference.samples);
|
||||
calibrated_s21_references_by_combo_.insert_or_assign(combo, std::move(calibrated));
|
||||
}
|
||||
|
||||
calibrated_s11_references_by_combo_.clear();
|
||||
if (!raw_s11_reference_bundle_.is_enabled()) {
|
||||
return;
|
||||
}
|
||||
if (!calibration_master.has_s11_calibration()) {
|
||||
throw std::runtime_error("S11 reference bundle requires S11 calibration bundle");
|
||||
}
|
||||
|
||||
calibrated_s11_references_by_combo_.reserve(combos.size());
|
||||
for (const auto& combo : combos) {
|
||||
const auto& raw_reference = raw_s11_reference_bundle_.resolve(combo);
|
||||
ChannelTrace calibrated{};
|
||||
calibrated.frequency_hz = raw_reference.frequency_hz;
|
||||
calibrated.samples = calibration_master.apply_s11(combo, raw_reference.frequency_hz, raw_reference.samples);
|
||||
calibrated_s11_references_by_combo_.insert_or_assign(combo, std::move(calibrated));
|
||||
}
|
||||
}
|
||||
|
||||
void ReferenceMaster::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
if (calibrated_s21_references_by_combo_.empty()) {
|
||||
throw std::runtime_error(
|
||||
"Calibrated references are not prepared. Call ReferenceMaster::prepare_calibrated() at startup."
|
||||
);
|
||||
}
|
||||
|
||||
raw_s21_reference_bundle_.validate_combos(combos);
|
||||
for (const auto& combo : combos) {
|
||||
if (!calibrated_s21_references_by_combo_.contains(combo)) {
|
||||
throw std::runtime_error("Calibrated S21 reference data is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
|
||||
raw_s11_reference_bundle_.validate_combos(combos);
|
||||
if (!raw_s11_reference_bundle_.is_enabled()) {
|
||||
return;
|
||||
}
|
||||
if (calibrated_s11_references_by_combo_.empty()) {
|
||||
throw std::runtime_error("Calibrated S11 references are not prepared");
|
||||
}
|
||||
for (const auto& combo : combos) {
|
||||
if (!calibrated_s11_references_by_combo_.contains(combo)) {
|
||||
throw std::runtime_error("Calibrated S11 reference data is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto ReferenceMaster::apply_to_trace(const ipc::SweepTraceBlock& calibrated_trace) const -> ipc::SweepTraceBlock {
|
||||
ipc::SweepTraceBlock output{};
|
||||
output.combo = calibrated_trace.combo;
|
||||
output.frequency_hz = calibrated_trace.frequency_hz;
|
||||
output.s21 = apply_s21(calibrated_trace.combo, calibrated_trace.frequency_hz, calibrated_trace.s21);
|
||||
output.s11 = apply_s11(calibrated_trace.combo, calibrated_trace.frequency_hz, calibrated_trace.s11);
|
||||
// Reference subtraction reshapes the samples, not when they were measured.
|
||||
output.capture_start_ns = calibrated_trace.capture_start_ns;
|
||||
output.capture_end_ns = calibrated_trace.capture_end_ns;
|
||||
return output;
|
||||
}
|
||||
|
||||
auto ReferenceMaster::apply_s21(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& calibrated_s21
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
const auto found = calibrated_s21_references_by_combo_.find(combo);
|
||||
if (found == calibrated_s21_references_by_combo_.end()) {
|
||||
throw std::runtime_error("Calibrated S21 reference is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
|
||||
const auto& reference = found->second;
|
||||
validate_channel_trace_layout(reference, "Calibrated S21 reference", combo);
|
||||
if (calibrated_s21.size() != reference.samples.size()) {
|
||||
throw std::runtime_error("S21 reference point count mismatch for combo " + combo_to_string(combo));
|
||||
}
|
||||
if (!frequency_axes_match(frequency_hz, reference.frequency_hz)) {
|
||||
throw std::runtime_error("S21 reference frequency axis mismatch for combo " + combo_to_string(combo));
|
||||
}
|
||||
|
||||
std::vector<ipc::Complex32> output(calibrated_s21.size());
|
||||
|
||||
static_assert(sizeof(ipc::Complex32) == sizeof(float) * 2U, "Complex32 layout must be two contiguous floats");
|
||||
using InterleavedComplexView = Eigen::Matrix<float, Eigen::Dynamic, 2, Eigen::RowMajor>;
|
||||
const auto point_count = static_cast<Eigen::Index>(calibrated_s21.size());
|
||||
|
||||
Eigen::Map<const InterleavedComplexView> calibrated_view(
|
||||
reinterpret_cast<const float*>(calibrated_s21.data()),
|
||||
point_count,
|
||||
2
|
||||
);
|
||||
Eigen::Map<const InterleavedComplexView> reference_view(
|
||||
reinterpret_cast<const float*>(reference.samples.data()),
|
||||
point_count,
|
||||
2
|
||||
);
|
||||
Eigen::Map<InterleavedComplexView> output_view(reinterpret_cast<float*>(output.data()), point_count, 2);
|
||||
|
||||
output_view = calibrated_view - reference_view;
|
||||
return output;
|
||||
}
|
||||
|
||||
auto ReferenceMaster::apply_s11(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
const std::vector<ipc::Complex32>& calibrated_s11
|
||||
) const -> std::vector<ipc::Complex32> {
|
||||
if (!raw_s11_reference_bundle_.is_enabled()) {
|
||||
return calibrated_s11;
|
||||
}
|
||||
|
||||
const auto found = calibrated_s11_references_by_combo_.find(combo);
|
||||
if (found == calibrated_s11_references_by_combo_.end()) {
|
||||
throw std::runtime_error("Calibrated S11 reference is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
|
||||
const auto& reference = found->second;
|
||||
validate_channel_trace_layout(reference, "Calibrated S11 reference", combo);
|
||||
if (calibrated_s11.size() != reference.samples.size()) {
|
||||
throw std::runtime_error("S11 reference point count mismatch for combo " + combo_to_string(combo));
|
||||
}
|
||||
if (!frequency_axes_match(frequency_hz, reference.frequency_hz)) {
|
||||
throw std::runtime_error("S11 reference frequency axis mismatch for combo " + combo_to_string(combo));
|
||||
}
|
||||
|
||||
std::vector<ipc::Complex32> output(calibrated_s11.size());
|
||||
for (std::size_t index = 0; index < calibrated_s11.size(); ++index) {
|
||||
output[index].re = calibrated_s11[index].re - reference.samples[index].re;
|
||||
output[index].im = calibrated_s11[index].im - reference.samples[index].im;
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
@@ -2,6 +2,7 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@@ -37,23 +38,21 @@ class DataProcessor {
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection;
|
||||
|
||||
[[nodiscard]] auto requested_mode(const ProcessingLiveConfig& live_config) const -> const std::string&;
|
||||
[[nodiscard]] auto resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface&;
|
||||
[[nodiscard]] auto should_replay_entire_history(const ProcessingLiveConfig& live_config) const -> bool;
|
||||
// Only gpr and sar3d use the speed, so only they recompute when a locator client changes it.
|
||||
[[nodiscard]] auto uses_socket_speed(const ProcessingLiveConfig& live_config) const -> bool;
|
||||
|
||||
// Merge live config with the latest socket-supplied speed (if any and if
|
||||
// not suppressed by `ignore_socket_speed`). The returned config is what
|
||||
// actually drives processing for this tick.
|
||||
[[nodiscard]] auto resolve_effective_live_config(const ProcessingLiveConfig& live_config) const
|
||||
-> ProcessingLiveConfig;
|
||||
// Speed streamed by a locator client (`vlc`), which then replaces
|
||||
// `gpr_speed_m_s`; nullopt when `ignore_socket_speed` is set or no fresh
|
||||
// value exists, so the speed from the live config stays in force.
|
||||
[[nodiscard]] auto socket_speed(const ProcessingLiveConfig& live_config) const -> std::optional<double>;
|
||||
|
||||
// Convert one processed collection into a locator filter spec, derived
|
||||
// from the live config and current processor mode.
|
||||
[[nodiscard]] auto build_locator_filter(const ProcessingLiveConfig& live_config) const
|
||||
-> radar::locator::FilterParams;
|
||||
|
||||
void publish_locator(const ipc::ResultCollection& collection, const ProcessingLiveConfig& live_config);
|
||||
void publish_locator(const ipc::ResultCollection& collection);
|
||||
|
||||
const config::RunConfig& config_;
|
||||
config::RunConfig processing_config_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing& results_ring_;
|
||||
ProcessorRegistry processors_{};
|
||||
|
||||
+39
-48
@@ -2,83 +2,52 @@
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
enum class HistoryCommand {
|
||||
None,
|
||||
RemoveLast,
|
||||
ClearAll,
|
||||
};
|
||||
|
||||
enum class LegacyGprMode {
|
||||
Point,
|
||||
Extended,
|
||||
};
|
||||
|
||||
enum class LegacyGprReferenceMode {
|
||||
// t_ref = midpoint between the first and last event centers.
|
||||
FrameCenter,
|
||||
// t_ref = center of the first event. Useful when motion offsets should be
|
||||
// accumulated from frame start, e.g. for tagging frames by their head time.
|
||||
FirstTxEvent,
|
||||
struct HistoryTarget {
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t capture_start_ns = 0;
|
||||
};
|
||||
|
||||
struct ProcessingLiveConfig {
|
||||
std::string processor_mode = "pass_through";
|
||||
std::string pass_through_channel = "s21";
|
||||
bool pass_through_fixed_y_enabled = false;
|
||||
float pass_through_y_min_db = -100.0F;
|
||||
float pass_through_y_max_db = 0.0F;
|
||||
std::string bscan_axis = "abs";
|
||||
std::string bscan_channel = "s21";
|
||||
float bscan_cut_m = 0.824F;
|
||||
float bscan_max_depth_m = 1.0F;
|
||||
float bscan_gain = 1.0F;
|
||||
float bscan_start_freq_mhz = 100.0F;
|
||||
float bscan_stop_freq_mhz = 8800.0F;
|
||||
LegacyGprMode legacy_gpr_mode = LegacyGprMode::Point;
|
||||
// Absent geometry overrides use the initial run_config geometry.
|
||||
std::optional<config::GprConfig> gpr{};
|
||||
std::vector<std::uint32_t> gpr_input_positions{};
|
||||
std::vector<std::uint32_t> gpr_output_positions{};
|
||||
float gpr_min_depth_m = 2.0F;
|
||||
float gpr_max_depth_m = 14.0F;
|
||||
float gpr_range_comp_power = 0.1F;
|
||||
float gpr_comp_power = 0.2F;
|
||||
// BP object-detection stop level, as a fraction of the global peak (Python
|
||||
// Horns_motion_3libre.py BP_OBJECT_MIN_FRAC); peaks below it are not objects.
|
||||
// BP object-detection stop level, as a fraction of the global peak; peaks
|
||||
// below it are not objects.
|
||||
float gpr_object_min_frac = 0.7F;
|
||||
std::string gpr_score_mode = "combined";
|
||||
// Backprojection intra-sweep speed-correction mode: "int_minus" (full
|
||||
// correction) or "int_focus" (focusing residual only). Mirrors the Python
|
||||
// Horns_motion_3libre.py MOTION_CORRECTION_MODE selector.
|
||||
// correction) or "int_focus" (focusing residual only).
|
||||
std::string gpr_motion_mode = "int_minus";
|
||||
float gpr_speed_m_s = 0.0F;
|
||||
float gpr_look_angle_deg = 0.0F;
|
||||
// Motion model parameters for the legacy GPR pipeline. The direction sign
|
||||
// selects which way later Tx-events appear deeper (+1) or shallower (-1).
|
||||
// Intra-sweep phase correction compensates the motion that happens *inside*
|
||||
// one Tx-sweep before the IFFT — it is independent of the per-pair coarse
|
||||
// tau shift and can be disabled without affecting the rest of the pipeline.
|
||||
// The reference mode picks the anchor used to compute dt_ref per event.
|
||||
float gpr_direction_sign = 1.0F;
|
||||
bool gpr_apply_freq_phase_correction = true;
|
||||
LegacyGprReferenceMode gpr_reference_mode = LegacyGprReferenceMode::FrameCenter;
|
||||
float gpr_snr_thresh = 4.5F;
|
||||
float gpr_snr_comp_max = 25.0F;
|
||||
float gpr_start_freq_mhz = 3000.0F;
|
||||
float gpr_stop_freq_mhz = 6000.0F;
|
||||
bool gpr_background_subtract_enabled = true;
|
||||
std::uint32_t gpr_background_mean_count = 10U;
|
||||
bool gpr_remove_sidelobe_objects_enabled = true;
|
||||
// BP image is computed in the y=imaging_plane_y_m slice of the 3D grid.
|
||||
// Default 0 keeps legacy 1D antenna layouts imaging in the antenna plane.
|
||||
float gpr_imaging_plane_y_m = 0.0F;
|
||||
// Coherent BP object visibility (window + the draw limits below) is applied in
|
||||
// the processor itself, matching Horns_motion_3libre.py — there is NO score
|
||||
// threshold for it. Only legacy GPR still thresholds, on a pair count.
|
||||
float legacy_gpr_min_visible_pair_count = 0.0F;
|
||||
// Ground plane in radar coordinates, at this height and the configured pitch.
|
||||
float gpr_bp_ground_depth_m = 1.25F;
|
||||
bool pair_depth_equalize = true;
|
||||
std::uint32_t gpr_max_detected_objects_to_draw = 0;
|
||||
std::uint32_t gpr_draw_top_m_objects = 0;
|
||||
// Cross-frame approach filter: an object is shown only once it persists as a
|
||||
@@ -90,18 +59,39 @@ struct ProcessingLiveConfig {
|
||||
float gpr_visible_x_max_m = 2.0F;
|
||||
float gpr_visible_z_min_m = 0.0F;
|
||||
float gpr_visible_z_max_m = 14.0F;
|
||||
// 3D SAR (sar3d processor) parameters, named after the reference notebook
|
||||
// Multi_depthnorm_clean.ipynb (SAR_FRAME_RADIUS, BP_GROUND_DEPTH_M, ...).
|
||||
// The band, depth gate, antenna selection, speed and look angle are the
|
||||
// shared gpr_* fields above.
|
||||
std::uint32_t sar3d_frame_radius = 0U;
|
||||
// SAR_VOXEL_M: the voxel steps along X, Y and Z.
|
||||
float sar3d_voxel_x_m = 0.025F;
|
||||
float sar3d_voxel_y_m = 0.02F;
|
||||
float sar3d_voxel_z_m = 0.02F;
|
||||
float sar3d_bp_ground_depth_m = 1.25F;
|
||||
float sar3d_comp_range_power = 0.3F;
|
||||
bool sar3d_use_compensation = true;
|
||||
bool sar3d_normalize_each_frame = true;
|
||||
float sar3d_x_size_m = 0.0F;
|
||||
float sar3d_y_min_m = -0.5F;
|
||||
float sar3d_y_max_m = 2.0F;
|
||||
float sar3d_z_size_m = 0.0F;
|
||||
bool sar3d_bg_gate_enabled = true; // BG_GATE_ENABLE: peak-to-background ratio for the display
|
||||
bool sar3d_clean_enabled = true; // SAR_CLEAN_ENABLE: CLEAN of a one-frame aperture into objects
|
||||
// When true, the data_processor ignores socket-supplied `vlc` updates and
|
||||
// keeps using `gpr_speed_m_s` from this file. Mirrored from the GUI's
|
||||
// "ignore socket speed" checkbox.
|
||||
bool ignore_socket_speed = false;
|
||||
bool reprocess_current_result = true;
|
||||
std::uint64_t history_command_seq = 0;
|
||||
HistoryCommand history_command = HistoryCommand::None;
|
||||
// Persistent desired history: normal settings writes must preserve deletions.
|
||||
std::uint64_t history_clear_before_ns = 0;
|
||||
std::vector<HistoryTarget> history_removed{};
|
||||
std::uint64_t settings_revision = 0;
|
||||
};
|
||||
|
||||
class ProcessingLiveConfigLoader {
|
||||
public:
|
||||
explicit ProcessingLiveConfigLoader(std::string path);
|
||||
ProcessingLiveConfigLoader(std::string path, const config::RunConfig& run_config);
|
||||
|
||||
[[nodiscard]] auto current() const -> ProcessingLiveConfig;
|
||||
[[nodiscard]] auto refresh_if_needed() -> ProcessingLiveConfig;
|
||||
@@ -111,6 +101,7 @@ class ProcessingLiveConfigLoader {
|
||||
[[nodiscard]] auto read_file_text() const -> std::string;
|
||||
|
||||
std::string path_{};
|
||||
const config::RunConfig& run_config_;
|
||||
ProcessingLiveConfig current_{};
|
||||
std::string last_json_text_{};
|
||||
bool has_last_json_text_ = false;
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
#include "data_processor.hpp"
|
||||
#include "runtime_wait.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
@@ -7,12 +9,12 @@
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "bscan_processor.hpp"
|
||||
#include "gpr_processor.hpp"
|
||||
#include "passthrough_processor.hpp"
|
||||
#include "sar3d_processor.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
@@ -20,16 +22,26 @@ namespace {
|
||||
constexpr const char* kDefaultProcessorMode = "pass_through";
|
||||
constexpr std::size_t kBscanReplayWindow = 50U;
|
||||
|
||||
[[nodiscard]] auto replay_history_limit(const config::RunConfig& config) -> std::size_t {
|
||||
[[nodiscard]] auto processing_history_limit(const config::RunConfig& config, const ProcessingLiveConfig& live) -> std::size_t {
|
||||
const auto preprocessed_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.preprocessed.capacity));
|
||||
const auto results_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.results.capacity));
|
||||
return std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow});
|
||||
std::size_t required = 1U;
|
||||
if (live.processor_mode == "gpr" || live.processor_mode == "sar3d") {
|
||||
if (live.gpr_background_subtract_enabled) { required += live.gpr_background_mean_count; }
|
||||
if (live.processor_mode == "sar3d") { required += live.sar3d_frame_radius; }
|
||||
}
|
||||
return std::max(required, std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow}));
|
||||
}
|
||||
|
||||
void publish_result_collection(const ipc::ResultCollection& collection, ipc::ShmRing& results_ring) {
|
||||
void publish_result_collection(
|
||||
const ipc::ResultCollection& collection, ipc::ShmRing& results_ring, std::uint64_t& oversized_results
|
||||
) {
|
||||
const auto out_bytes = ipc::serialize_result_collection(collection);
|
||||
if (!results_ring.push(out_bytes)) {
|
||||
throw std::runtime_error("Results ring slot is too small for serialized collection");
|
||||
if (!results_ring.push(out_bytes) && ++oversized_results % 100U == 1U) {
|
||||
std::cerr << "data_processor: result " << collection.collection_id << " requires " << out_bytes.size()
|
||||
<< " bytes, results ring slot holds " << results_ring.slot_size_bytes()
|
||||
<< "; increase rings.results.slot_size_bytes or reduce processing output"
|
||||
<< " (dropped total=" << oversized_results << ")\n";
|
||||
}
|
||||
}
|
||||
|
||||
@@ -43,11 +55,12 @@ DataProcessor::DataProcessor(
|
||||
radar::locator::TcpServer* locator_server
|
||||
)
|
||||
: config_(config),
|
||||
processing_config_(config),
|
||||
preprocessed_ring_(preprocessed_ring),
|
||||
results_ring_(results_ring),
|
||||
processors_(std::move(processors)),
|
||||
default_processor_mode_(kDefaultProcessorMode),
|
||||
live_config_loader_(config.runtime.processing_live_config_path),
|
||||
live_config_loader_(config.runtime.processing_live_config_path, config),
|
||||
locator_server_(locator_server) {
|
||||
if (processors_.empty()) {
|
||||
throw std::runtime_error("DataProcessor requires at least one processor");
|
||||
@@ -60,120 +73,123 @@ DataProcessor::DataProcessor(
|
||||
void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
std::vector<std::uint8_t> bytes{};
|
||||
std::vector<ipc::PreprocessedCollection> preprocessed_history{};
|
||||
const std::size_t history_limit = replay_history_limit(config_);
|
||||
std::uint64_t last_replayed_revision = live_config_loader_.revision();
|
||||
std::uint64_t last_applied_history_command_seq = 0;
|
||||
std::uint64_t error_count = 0;
|
||||
std::uint64_t consecutive_errors = 0;
|
||||
// track the socket-fed speed used for the last reprocess so a change
|
||||
// arriving without a live-config revision bump still triggers a reprocess of
|
||||
// the current result (gated below by reprocess_current_result).
|
||||
runtime::CollectionErrors errors;
|
||||
std::uint64_t oversized_results = 0;
|
||||
// The socket-fed speed used by the last reprocess: in the modes that use the speed, a
|
||||
// change without a live-config revision still recomputes the current result.
|
||||
std::optional<double> last_reprocessed_socket_speed = std::nullopt;
|
||||
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
try {
|
||||
const auto live_config_raw = live_config_loader_.refresh_if_needed();
|
||||
const auto live_config = resolve_effective_live_config(live_config_raw);
|
||||
// The socket speed is read once per tick: it both drives this tick's
|
||||
// processing and, when it changed, triggers a reprocess on its own,
|
||||
// even when the live revision is unchanged.
|
||||
auto live_config = live_config_loader_.refresh_if_needed();
|
||||
const std::optional<double> current_socket_speed = socket_speed(live_config);
|
||||
if (current_socket_speed.has_value()) {
|
||||
live_config.gpr_speed_m_s = static_cast<float>(*current_socket_speed);
|
||||
}
|
||||
const auto live_revision = live_config_loader_.revision();
|
||||
if (live_revision != last_replayed_revision) {
|
||||
processing_config_.gpr = live_config.gpr.value_or(config_.gpr);
|
||||
}
|
||||
auto& processor = resolve_processor(live_config);
|
||||
const auto history_limit = processing_history_limit(config_, live_config);
|
||||
const bool socket_speed_dirty =
|
||||
uses_socket_speed(live_config) && current_socket_speed != last_reprocessed_socket_speed;
|
||||
|
||||
// The effective config folds in latest_socket_speed(); a change there is
|
||||
// a reprocess trigger on its own, even when the live revision is unchanged.
|
||||
// Mirror the gating in resolve_effective_live_config(): only consider the
|
||||
// socket speed when it is actually applied to the effective config.
|
||||
const std::optional<double> current_socket_speed =
|
||||
(live_config_raw.ignore_socket_speed || locator_server_ == nullptr)
|
||||
? std::nullopt
|
||||
: locator_server_->latest_socket_speed();
|
||||
const bool socket_speed_dirty = current_socket_speed != last_reprocessed_socket_speed;
|
||||
|
||||
const auto was_removed = [&](const ipc::PreprocessedCollection& collection) {
|
||||
if (live_config.history_clear_before_ns &&
|
||||
collection.capture_start_ns <= live_config.history_clear_before_ns) {
|
||||
return true;
|
||||
}
|
||||
return std::any_of(live_config.history_removed.begin(), live_config.history_removed.end(),
|
||||
[&](const HistoryTarget& target) {
|
||||
return collection.collection_id == target.collection_id &&
|
||||
collection.capture_start_ns == target.capture_start_ns;
|
||||
});
|
||||
};
|
||||
if (live_revision != last_replayed_revision || socket_speed_dirty) {
|
||||
if (live_config.history_command_seq > last_applied_history_command_seq) {
|
||||
if (live_config.history_command == HistoryCommand::RemoveLast) {
|
||||
if (!preprocessed_history.empty()) {
|
||||
preprocessed_history.pop_back();
|
||||
}
|
||||
} else if (live_config.history_command == HistoryCommand::ClearAll) {
|
||||
preprocessed_history.clear();
|
||||
}
|
||||
last_applied_history_command_seq = live_config.history_command_seq;
|
||||
// Settled before recomputing: a sweep that fails is reported once rather than
|
||||
// retried every tick, which would stop all new input.
|
||||
last_replayed_revision = live_revision;
|
||||
last_reprocessed_socket_speed = current_socket_speed;
|
||||
std::erase_if(preprocessed_history, was_removed);
|
||||
if (preprocessed_history.size() > history_limit) {
|
||||
preprocessed_history.erase(preprocessed_history.begin(), preprocessed_history.end() - history_limit);
|
||||
}
|
||||
|
||||
if (!live_config.reprocess_current_result) {
|
||||
// Socket-fed speed updates should affect only future preprocessed collections,
|
||||
// not replay the current history entry.
|
||||
} else if (should_replay_entire_history(live_config)) {
|
||||
for (std::size_t index = 0; index < preprocessed_history.size(); ++index) {
|
||||
const auto replay_result = process_collection(
|
||||
preprocessed_history[index],
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.data(), index),
|
||||
processor,
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
publish_locator(replay_result, live_config);
|
||||
// The GUI turns this off while it re-feeds the sweeps of a stopped B-scan itself.
|
||||
if (live_config.reprocess_current_result && !preprocessed_history.empty()) {
|
||||
const auto newest = preprocessed_history.size() - 1U;
|
||||
const auto first = should_replay_entire_history(live_config) ? std::size_t{0} : newest;
|
||||
for (auto index = first; index <= newest; ++index) {
|
||||
try {
|
||||
const auto replay_result = process_collection(
|
||||
preprocessed_history[index],
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.data(), index),
|
||||
processor,
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(replay_result, results_ring_, oversized_results);
|
||||
if (index == newest) {
|
||||
publish_locator(replay_result); // The locator reports the current scene.
|
||||
}
|
||||
} catch (const std::exception& exc) {
|
||||
errors.report("data_processor", exc);
|
||||
}
|
||||
}
|
||||
} else if (!preprocessed_history.empty()) {
|
||||
const auto replay_result = process_collection(
|
||||
preprocessed_history.back(),
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.data(), preprocessed_history.size() - 1U),
|
||||
processor,
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
publish_locator(replay_result, live_config);
|
||||
}
|
||||
last_replayed_revision = live_revision;
|
||||
//record the speed we just reprocessed with so an
|
||||
// unchanged socket value does not retrigger every iteration.
|
||||
last_reprocessed_socket_speed = current_socket_speed;
|
||||
}
|
||||
|
||||
if (preprocessed_ring_.pop(bytes)) {
|
||||
auto preprocessed = ipc::deserialize_preprocessed_collection(bytes);
|
||||
preprocessed_history.push_back(std::move(preprocessed));
|
||||
while (preprocessed_history.size() > history_limit) {
|
||||
preprocessed_history.erase(preprocessed_history.begin());
|
||||
if (!preprocessed.capture_start_ns || preprocessed.capture_end_ns < preprocessed.capture_start_ns) {
|
||||
throw std::runtime_error("Preprocessed collection requires a valid acquisition window");
|
||||
}
|
||||
if (was_removed(preprocessed)) {
|
||||
continue; // A deletion also applies to queued input and later GUI replays.
|
||||
}
|
||||
// Sweeps the GUI feeds back to recompute its B-scan keep their original
|
||||
// timestamps, so they are no newer than the newest retained sweep. They
|
||||
// are processed like any other but stay out of the history, which must
|
||||
// remain the acquisition in order (later sweeps take their aperture and
|
||||
// background from it), and away from the locator, which reports the
|
||||
// current scene.
|
||||
const bool fed_back = !preprocessed_history.empty() &&
|
||||
preprocessed.capture_start_ns <= preprocessed_history.back().capture_start_ns;
|
||||
if (!fed_back) {
|
||||
preprocessed_history.push_back(std::move(preprocessed));
|
||||
if (preprocessed_history.size() > history_limit) {
|
||||
preprocessed_history.erase(preprocessed_history.begin(), preprocessed_history.end() - history_limit);
|
||||
}
|
||||
}
|
||||
const auto& current = fed_back ? preprocessed : preprocessed_history.back();
|
||||
const auto earlier = std::partition_point(
|
||||
preprocessed_history.begin(),
|
||||
preprocessed_history.end(),
|
||||
[&](const ipc::PreprocessedCollection& retained) { return retained.capture_start_ns < current.capture_start_ns; }
|
||||
);
|
||||
|
||||
const auto result_collection = process_collection(
|
||||
preprocessed_history.back(),
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.data(), preprocessed_history.size() - 1U),
|
||||
current,
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.begin(), earlier),
|
||||
processor,
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(result_collection, results_ring_);
|
||||
publish_locator(result_collection, live_config);
|
||||
consecutive_errors = 0; // made progress; reset the escalation counter
|
||||
publish_result_collection(result_collection, results_ring_, oversized_results);
|
||||
if (!fed_back) {
|
||||
publish_locator(result_collection);
|
||||
}
|
||||
errors.progress();
|
||||
continue;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.runtime.idle_sleep_ms));
|
||||
(void)config::wait_or_stop(config_.runtime.idle_sleep_ms, stop_requested);
|
||||
} catch (const std::exception& exc) {
|
||||
// A ring-slot-too-small failure is a permanent config error (the slot
|
||||
// cannot hold a serialized collection): swallowing it would silently
|
||||
// drop every collection forever, so fail fast and let the operator fix it.
|
||||
if (std::string(exc.what()).find("slot is too small") != std::string::npos) {
|
||||
throw;
|
||||
}
|
||||
// A single bad collection (torn ring slot, decode/processing error) must
|
||||
// not kill the long-running processor: drop it and keep going.
|
||||
++error_count;
|
||||
++consecutive_errors;
|
||||
// Escalate a PERSISTENT failure (a deterministic config error drops every
|
||||
// collection forever): log it loudly and distinctly — not buried in the
|
||||
// every-100 throttle — so liveness-only supervision still surfaces it.
|
||||
if (consecutive_errors % 500 == 0) {
|
||||
std::cerr << "data_processor: " << consecutive_errors
|
||||
<< " consecutive failures — likely a permanent configuration error: "
|
||||
<< exc.what() << '\n';
|
||||
} else if (error_count % 100 == 1) {
|
||||
std::cerr << "data_processor: dropped collection after error (count="
|
||||
<< error_count << "): " << exc.what() << '\n';
|
||||
}
|
||||
// Back-off (floored at 1ms even if idle_sleep_ms==0) so a persistent error
|
||||
// cannot busy-spin a core or flood the log.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(std::max<int>(1, config_.runtime.idle_sleep_ms)));
|
||||
errors.report("data_processor", exc);
|
||||
(void)config::wait_or_stop(std::max(1U, config_.runtime.idle_sleep_ms), stop_requested);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -185,19 +201,27 @@ auto DataProcessor::process_collection(
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
const auto started_at = std::chrono::steady_clock::now();
|
||||
auto result = processor.process_collection(config_, preprocessed, previous_collections, live_config);
|
||||
auto result = processor.process_collection(processing_config_, preprocessed, previous_collections, live_config);
|
||||
const auto finished_at = std::chrono::steady_clock::now();
|
||||
result.processing_duration_ns = static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(finished_at - started_at).count()
|
||||
);
|
||||
result.collection_id = preprocessed.collection_id;
|
||||
result.monotonic_ns = ipc::current_monotonic_ns();
|
||||
result.settings_revision = live_config.settings_revision;
|
||||
result.capture_start_ns = preprocessed.capture_start_ns;
|
||||
result.capture_end_ns = preprocessed.capture_end_ns;
|
||||
// Socket speed can change without a GUI revision; retain the actual value per result.
|
||||
result.speed_m_s = live_config.gpr_speed_m_s;
|
||||
return result;
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface& {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
auto DataProcessor::requested_mode(const ProcessingLiveConfig& live_config) const -> const std::string& {
|
||||
return live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
}
|
||||
|
||||
if (auto found = processors_.find(requested_mode); found != processors_.end()) {
|
||||
auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface& {
|
||||
if (auto found = processors_.find(requested_mode(live_config)); found != processors_.end()) {
|
||||
return *(found->second);
|
||||
}
|
||||
if (auto fallback = processors_.find(default_processor_mode_); fallback != processors_.end()) {
|
||||
@@ -207,59 +231,28 @@ auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -
|
||||
}
|
||||
|
||||
auto DataProcessor::should_replay_entire_history(const ProcessingLiveConfig& live_config) const -> bool {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
return requested_mode == "bscan";
|
||||
return requested_mode(live_config) == "bscan";
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_effective_live_config(const ProcessingLiveConfig& live_config) const
|
||||
-> ProcessingLiveConfig {
|
||||
auto DataProcessor::uses_socket_speed(const ProcessingLiveConfig& live_config) const -> bool {
|
||||
const auto& mode = requested_mode(live_config);
|
||||
return mode == "gpr" || mode == "sar3d";
|
||||
}
|
||||
|
||||
auto DataProcessor::socket_speed(const ProcessingLiveConfig& live_config) const -> std::optional<double> {
|
||||
if (live_config.ignore_socket_speed || locator_server_ == nullptr) {
|
||||
return live_config;
|
||||
return std::nullopt;
|
||||
}
|
||||
const auto socket_speed = locator_server_->latest_socket_speed();
|
||||
if (!socket_speed.has_value()) {
|
||||
return live_config;
|
||||
}
|
||||
ProcessingLiveConfig effective = live_config;
|
||||
effective.gpr_speed_m_s = static_cast<float>(*socket_speed);
|
||||
return effective;
|
||||
}
|
||||
|
||||
auto DataProcessor::build_locator_filter(const ProcessingLiveConfig& live_config) const
|
||||
-> radar::locator::FilterParams {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
|
||||
radar::locator::FilterParams filter{};
|
||||
if (requested_mode == "legacy_gpr") {
|
||||
// Legacy GPR emits its objects unfiltered, so the socket applies the legacy
|
||||
// rule here: clip to the visible window and threshold on the pair count. The
|
||||
// GUI disables "draw top N" for legacy, so we skip it on the wire to match.
|
||||
filter.visible_bounds = radar::locator::VisibleBounds{
|
||||
.x_min = live_config.gpr_visible_x_min_m,
|
||||
.x_max = live_config.gpr_visible_x_max_m,
|
||||
.z_min = live_config.gpr_visible_z_min_m,
|
||||
.z_max = live_config.gpr_visible_z_max_m,
|
||||
};
|
||||
filter.min_score = live_config.legacy_gpr_min_visible_pair_count;
|
||||
filter.draw_limits.reset();
|
||||
}
|
||||
// Coherent BP already emits the FINAL visible object set from the processor
|
||||
// (window + N/M, no score threshold — matching Horns_motion_3libre.py), so the
|
||||
// socket forwards it verbatim. The default FilterParams passes everything through
|
||||
// (it only drops non-finite rows), keeping the filtering logic in one place.
|
||||
return filter;
|
||||
return locator_server_->latest_socket_speed();
|
||||
}
|
||||
|
||||
void DataProcessor::publish_locator(
|
||||
const ipc::ResultCollection& collection,
|
||||
const ProcessingLiveConfig& live_config
|
||||
const ipc::ResultCollection& collection
|
||||
) {
|
||||
if (locator_server_ == nullptr || !locator_server_->is_running()) {
|
||||
if (locator_server_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
locator_server_->publish(collection, build_locator_filter(live_config));
|
||||
locator_server_->publish(collection);
|
||||
}
|
||||
|
||||
auto create_default_processors() -> ProcessorRegistry {
|
||||
@@ -277,7 +270,7 @@ auto create_default_processors() -> ProcessorRegistry {
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
{
|
||||
auto processor = std::make_unique<LegacyGprProcessor>();
|
||||
auto processor = std::make_unique<Sar3dProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
return processors;
|
||||
|
||||
@@ -1,43 +1,14 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "data_processor.hpp"
|
||||
#include "locator/tcp_server.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kDefaultConfigPath = "run_config.json";
|
||||
|
||||
std::atomic<bool> g_stop_requested{false};
|
||||
|
||||
void signal_handler(int /*signal*/) {
|
||||
g_stop_requested.store(true, std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
void install_signal_handlers() {
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
// Writing to a peer-closed socket would otherwise terminate the process.
|
||||
std::signal(SIGPIPE, SIG_IGN);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_config_path(int argc, char** argv) -> std::string {
|
||||
std::string config_path = kDefaultConfigPath;
|
||||
for (int index = 1; index < argc; ++index) {
|
||||
const std::string arg = argv[index];
|
||||
if (arg == "--config" && (index + 1) < argc) {
|
||||
config_path = argv[++index];
|
||||
}
|
||||
}
|
||||
return config_path;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto start_locator_server(const radar::config::RunConfig& config)
|
||||
-> std::unique_ptr<radar::locator::TcpServer> {
|
||||
if (!config.locator_server.enabled) {
|
||||
@@ -56,11 +27,8 @@ void install_signal_handlers() {
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
install_signal_handlers();
|
||||
|
||||
try {
|
||||
const auto config_path = read_config_path(argc, argv);
|
||||
const auto config = radar::config::load_run_config(config_path);
|
||||
const auto config = radar::runtime::initialize(argc, argv);
|
||||
|
||||
auto preprocessed_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.preprocessed.name,
|
||||
@@ -82,7 +50,7 @@ int main(int argc, char** argv) {
|
||||
radar::processing::create_default_processors(),
|
||||
locator_server.get()
|
||||
);
|
||||
processor.run(g_stop_requested);
|
||||
processor.run(radar::runtime::stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_processor error: " << exception.what() << '\n';
|
||||
|
||||
+140
-270
@@ -9,6 +9,7 @@
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
@@ -18,41 +19,6 @@ namespace {
|
||||
|
||||
using Json = nlohmann::json;
|
||||
|
||||
[[nodiscard]] auto parse_history_command(const std::string& value) -> HistoryCommand {
|
||||
if (value == "none") {
|
||||
return HistoryCommand::None;
|
||||
}
|
||||
if (value == "remove_last") {
|
||||
return HistoryCommand::RemoveLast;
|
||||
}
|
||||
if (value == "clear_all") {
|
||||
return HistoryCommand::ClearAll;
|
||||
}
|
||||
throw std::runtime_error("processing.history_command must be one of: none, remove_last, clear_all");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_legacy_gpr_mode(const std::string& value, const std::string& field_name) -> LegacyGprMode {
|
||||
if (value == "point") {
|
||||
return LegacyGprMode::Point;
|
||||
}
|
||||
if (value == "extended") {
|
||||
return LegacyGprMode::Extended;
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: point, extended");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_legacy_gpr_reference_mode(
|
||||
const std::string& value, const std::string& field_name
|
||||
) -> LegacyGprReferenceMode {
|
||||
if (value == "frame_center") {
|
||||
return LegacyGprReferenceMode::FrameCenter;
|
||||
}
|
||||
if (value == "first_tx_event") {
|
||||
return LegacyGprReferenceMode::FirstTxEvent;
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: frame_center, first_tx_event");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_score_mode(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "peak" || value == "combined") {
|
||||
return value;
|
||||
@@ -67,41 +33,55 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error(field_name + " must be one of: int_minus, int_focus");
|
||||
}
|
||||
|
||||
void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) {
|
||||
if (value == "backprojection") {
|
||||
return;
|
||||
[[nodiscard]] auto parse_f32(const Json& value, const std::string& field_name) -> float {
|
||||
if (!value.is_number()) {
|
||||
throw std::runtime_error(field_name + " must be number");
|
||||
}
|
||||
if (value == "legacy_point") {
|
||||
config.legacy_gpr_mode = LegacyGprMode::Point;
|
||||
} else if (value == "legacy_extended") {
|
||||
config.legacy_gpr_mode = LegacyGprMode::Extended;
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"processing.gpr_algorithm must be one of: backprojection, legacy_point, legacy_extended"
|
||||
);
|
||||
}
|
||||
if (config.processor_mode.empty() || config.processor_mode == "gpr") {
|
||||
config.processor_mode = "legacy_gpr";
|
||||
const auto number = value.get<double>();
|
||||
if (!std::isfinite(number) || std::fabs(number) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error(field_name + " must fit finite float32");
|
||||
}
|
||||
return static_cast<float>(number);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_s_parameter_channel(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "s21" || value == "s11") {
|
||||
return value;
|
||||
template <typename T>
|
||||
void assign_optional(const Json& root, const char* key, T& target) {
|
||||
const auto found = root.find(key);
|
||||
if (found == root.end()) {
|
||||
return;
|
||||
}
|
||||
const auto field_name = std::string("processing.") + key;
|
||||
if constexpr (std::is_same_v<T, float>) {
|
||||
target = parse_f32(*found, field_name);
|
||||
} else {
|
||||
const bool valid = std::is_same_v<T, bool> ? found->is_boolean() : found->is_string();
|
||||
if (!valid) {
|
||||
throw std::runtime_error(field_name + " has invalid type");
|
||||
}
|
||||
target = found->get<T>();
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: s21, s11");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_u64_number(const Json& value, const std::string& field_name) -> std::uint64_t {
|
||||
if (!value.is_number()) {
|
||||
throw std::runtime_error(field_name + " must be number");
|
||||
}
|
||||
if (value.is_number_unsigned()) {
|
||||
return value.get<std::uint64_t>();
|
||||
}
|
||||
if (value.is_number_integer()) {
|
||||
const auto integer = value.get<std::int64_t>();
|
||||
if (integer < 0) {
|
||||
throw std::runtime_error(field_name + " is out of uint64 range");
|
||||
}
|
||||
return static_cast<std::uint64_t>(integer);
|
||||
}
|
||||
const double numeric = value.get<double>();
|
||||
if (numeric < 0.0 || numeric > static_cast<double>(std::numeric_limits<std::uint64_t>::max())) {
|
||||
if (!std::isfinite(numeric) || numeric < 0.0 || numeric >= static_cast<double>(std::numeric_limits<std::uint64_t>::max())) {
|
||||
throw std::runtime_error(field_name + " is out of uint64 range");
|
||||
}
|
||||
const double rounded = std::round(numeric);
|
||||
if (std::fabs(numeric - rounded) > 1e-6) {
|
||||
if (numeric != rounded) {
|
||||
throw std::runtime_error(field_name + " must be integer");
|
||||
}
|
||||
return static_cast<std::uint64_t>(rounded);
|
||||
@@ -128,7 +108,31 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
return result;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_live_config(const std::string& json_text, const std::string& path) -> ProcessingLiveConfig {
|
||||
template <typename Entry>
|
||||
auto parse_geometry(const Json& value, const char* position_field, std::uint32_t Entry::*position_member)
|
||||
-> std::vector<Entry> {
|
||||
if (!value.is_array()) {
|
||||
throw std::runtime_error("processing geometry must be an array");
|
||||
}
|
||||
std::vector<Entry> entries{};
|
||||
entries.reserve(value.size());
|
||||
for (const auto& record : value) {
|
||||
if (!record.is_object()) {
|
||||
throw std::runtime_error("processing geometry entry must be an object");
|
||||
}
|
||||
Entry entry{};
|
||||
entry.*position_member = parse_u32_number(record.at(position_field), position_field);
|
||||
entry.x_m = parse_f32(record.at("x_m"), "processing.x_m");
|
||||
assign_optional(record, "y_m", entry.y_m);
|
||||
assign_optional(record, "z_m", entry.z_m);
|
||||
entries.push_back(entry);
|
||||
}
|
||||
return entries;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_live_config(
|
||||
const std::string& json_text, const std::string& path, const config::RunConfig& run_config
|
||||
) -> ProcessingLiveConfig {
|
||||
if (json_text.empty()) {
|
||||
throw std::runtime_error("Processing live config is empty: " + path);
|
||||
}
|
||||
@@ -145,128 +149,40 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
}
|
||||
|
||||
ProcessingLiveConfig config{};
|
||||
if (const auto found = root.find("processor_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.processor_mode must be string");
|
||||
}
|
||||
config.processor_mode = found->get<std::string>();
|
||||
if (const auto found = root.find("settings_revision"); found != root.end()) {
|
||||
config.settings_revision = parse_u64_number(*found, "processing.settings_revision");
|
||||
}
|
||||
if (const auto found = root.find("pass_through_channel"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.pass_through_channel must be string");
|
||||
if (root.contains("tx_geometry") || root.contains("rx_geometry")) {
|
||||
config.gpr = run_config.gpr;
|
||||
if (const auto found = root.find("tx_geometry"); found != root.end()) {
|
||||
config.gpr->tx_geometry = parse_geometry(*found, "output_pos", &config::GprTxGeometry::output_pos);
|
||||
}
|
||||
config.pass_through_channel =
|
||||
parse_s_parameter_channel(found->get<std::string>(), "processing.pass_through_channel");
|
||||
}
|
||||
if (const auto found = root.find("pass_through_fixed_y_enabled"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.pass_through_fixed_y_enabled must be bool");
|
||||
if (const auto found = root.find("rx_geometry"); found != root.end()) {
|
||||
config.gpr->rx_geometry = parse_geometry(*found, "input_pos", &config::GprRxGeometry::input_pos);
|
||||
}
|
||||
config.pass_through_fixed_y_enabled = found->get<bool>();
|
||||
config::validate_gpr_config(*config.gpr, run_config);
|
||||
}
|
||||
if (const auto found = root.find("pass_through_y_min_db"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.pass_through_y_min_db must be number");
|
||||
}
|
||||
config.pass_through_y_min_db = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("pass_through_y_max_db"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.pass_through_y_max_db must be number");
|
||||
}
|
||||
config.pass_through_y_max_db = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("bscan_axis"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.bscan_axis must be string");
|
||||
}
|
||||
config.bscan_axis = found->get<std::string>();
|
||||
}
|
||||
if (const auto found = root.find("bscan_channel"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.bscan_channel must be string");
|
||||
}
|
||||
config.bscan_channel = parse_s_parameter_channel(found->get<std::string>(), "processing.bscan_channel");
|
||||
}
|
||||
if (const auto found = root.find("bscan_cut_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.bscan_cut_m must be number");
|
||||
}
|
||||
config.bscan_cut_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("bscan_max_depth_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.bscan_max_depth_m must be number");
|
||||
}
|
||||
config.bscan_max_depth_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("bscan_gain"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.bscan_gain must be number");
|
||||
}
|
||||
config.bscan_gain = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("bscan_start_freq_mhz"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.bscan_start_freq_mhz must be number");
|
||||
}
|
||||
config.bscan_start_freq_mhz = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("bscan_stop_freq_mhz"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.bscan_stop_freq_mhz must be number");
|
||||
}
|
||||
config.bscan_stop_freq_mhz = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("legacy_gpr_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.legacy_gpr_mode must be string");
|
||||
}
|
||||
config.legacy_gpr_mode =
|
||||
parse_legacy_gpr_mode(found->get<std::string>(), "processing.legacy_gpr_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_algorithm"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_algorithm must be string");
|
||||
}
|
||||
apply_legacy_gpr_algorithm_alias(config, found->get<std::string>());
|
||||
assign_optional(root, "processor_mode", config.processor_mode);
|
||||
if (!config.processor_mode.empty() && config.processor_mode != "pass_through"
|
||||
&& config.processor_mode != "bscan" && config.processor_mode != "gpr" && config.processor_mode != "sar3d") {
|
||||
throw std::runtime_error("Unsupported processing.processor_mode: " + config.processor_mode);
|
||||
}
|
||||
assign_optional(root, "bscan_axis", config.bscan_axis);
|
||||
assign_optional(root, "bscan_cut_m", config.bscan_cut_m);
|
||||
assign_optional(root, "bscan_max_depth_m", config.bscan_max_depth_m);
|
||||
assign_optional(root, "bscan_gain", config.bscan_gain);
|
||||
assign_optional(root, "bscan_start_freq_mhz", config.bscan_start_freq_mhz);
|
||||
assign_optional(root, "bscan_stop_freq_mhz", config.bscan_stop_freq_mhz);
|
||||
if (const auto found = root.find("gpr_input_positions"); found != root.end()) {
|
||||
config.gpr_input_positions = parse_u32_array(*found, "processing.gpr_input_positions");
|
||||
}
|
||||
if (const auto found = root.find("gpr_output_positions"); found != root.end()) {
|
||||
config.gpr_output_positions = parse_u32_array(*found, "processing.gpr_output_positions");
|
||||
}
|
||||
if (const auto found = root.find("gpr_min_depth_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_min_depth_m must be number");
|
||||
}
|
||||
config.gpr_min_depth_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_max_depth_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_max_depth_m must be number");
|
||||
}
|
||||
config.gpr_max_depth_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_range_comp_power"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_range_comp_power must be number");
|
||||
}
|
||||
config.gpr_range_comp_power = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_object_min_frac"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_object_min_frac must be number");
|
||||
}
|
||||
config.gpr_object_min_frac = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_comp_power"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_comp_power must be number");
|
||||
}
|
||||
config.gpr_comp_power = static_cast<float>(found->get<double>());
|
||||
}
|
||||
assign_optional(root, "gpr_min_depth_m", config.gpr_min_depth_m);
|
||||
assign_optional(root, "gpr_max_depth_m", config.gpr_max_depth_m);
|
||||
assign_optional(root, "gpr_range_comp_power", config.gpr_range_comp_power);
|
||||
assign_optional(root, "gpr_object_min_frac", config.gpr_object_min_frac);
|
||||
if (const auto found = root.find("gpr_score_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_score_mode must be string");
|
||||
@@ -279,100 +195,25 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
}
|
||||
config.gpr_motion_mode = parse_gpr_motion_mode(found->get<std::string>(), "processing.gpr_motion_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_speed_m_s"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_speed_m_s must be number");
|
||||
}
|
||||
config.gpr_speed_m_s = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_look_angle_deg"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_look_angle_deg must be number");
|
||||
}
|
||||
config.gpr_look_angle_deg = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_direction_sign"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_direction_sign must be number");
|
||||
}
|
||||
config.gpr_direction_sign = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_apply_freq_phase_correction"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.gpr_apply_freq_phase_correction must be bool");
|
||||
}
|
||||
config.gpr_apply_freq_phase_correction = found->get<bool>();
|
||||
}
|
||||
if (const auto found = root.find("gpr_reference_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_reference_mode must be string");
|
||||
}
|
||||
config.gpr_reference_mode =
|
||||
parse_legacy_gpr_reference_mode(found->get<std::string>(), "processing.gpr_reference_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_snr_thresh"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_snr_thresh must be number");
|
||||
}
|
||||
config.gpr_snr_thresh = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_snr_comp_max"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_snr_comp_max must be number");
|
||||
}
|
||||
config.gpr_snr_comp_max = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_start_freq_mhz"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_start_freq_mhz must be number");
|
||||
}
|
||||
config.gpr_start_freq_mhz = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_stop_freq_mhz"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_stop_freq_mhz must be number");
|
||||
}
|
||||
config.gpr_stop_freq_mhz = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_background_subtract_enabled"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.gpr_background_subtract_enabled must be bool");
|
||||
}
|
||||
config.gpr_background_subtract_enabled = found->get<bool>();
|
||||
}
|
||||
assign_optional(root, "gpr_speed_m_s", config.gpr_speed_m_s);
|
||||
assign_optional(root, "gpr_look_angle_deg", config.gpr_look_angle_deg);
|
||||
assign_optional(root, "gpr_direction_sign", config.gpr_direction_sign);
|
||||
assign_optional(root, "gpr_start_freq_mhz", config.gpr_start_freq_mhz);
|
||||
assign_optional(root, "gpr_stop_freq_mhz", config.gpr_stop_freq_mhz);
|
||||
assign_optional(root, "gpr_background_subtract_enabled", config.gpr_background_subtract_enabled);
|
||||
if (const auto found = root.find("gpr_background_mean_count"); found != root.end()) {
|
||||
config.gpr_background_mean_count = parse_u32_number(*found, "processing.gpr_background_mean_count");
|
||||
}
|
||||
if (const auto found = root.find("gpr_remove_sidelobe_objects_enabled"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.gpr_remove_sidelobe_objects_enabled must be bool");
|
||||
}
|
||||
config.gpr_remove_sidelobe_objects_enabled = found->get<bool>();
|
||||
}
|
||||
if (const auto found = root.find("gpr_imaging_plane_y_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_imaging_plane_y_m must be number");
|
||||
}
|
||||
config.gpr_imaging_plane_y_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
assign_optional(root, "gpr_remove_sidelobe_objects_enabled", config.gpr_remove_sidelobe_objects_enabled);
|
||||
assign_optional(root, "gpr_bp_ground_depth_m", config.gpr_bp_ground_depth_m);
|
||||
assign_optional(root, "pair_depth_equalize", config.pair_depth_equalize);
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"gpr_visible_x_min_m", &config.gpr_visible_x_min_m},
|
||||
std::pair{"gpr_visible_x_max_m", &config.gpr_visible_x_max_m},
|
||||
std::pair{"gpr_visible_z_min_m", &config.gpr_visible_z_min_m},
|
||||
std::pair{"gpr_visible_z_max_m", &config.gpr_visible_z_max_m},
|
||||
}) {
|
||||
if (const auto found = root.find(key); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error(std::string("processing.") + key + " must be number");
|
||||
}
|
||||
*target = static_cast<float>(found->get<double>());
|
||||
}
|
||||
}
|
||||
if (const auto found = root.find("legacy_gpr_min_visible_pair_count"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.legacy_gpr_min_visible_pair_count must be number");
|
||||
}
|
||||
config.legacy_gpr_min_visible_pair_count = static_cast<float>(found->get<double>());
|
||||
assign_optional(root, key, *target);
|
||||
}
|
||||
if (const auto found = root.find("gpr_max_detected_objects_to_draw"); found != root.end()) {
|
||||
config.gpr_max_detected_objects_to_draw =
|
||||
@@ -386,34 +227,62 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
config.gpr_object_approach_min_frames =
|
||||
parse_u32_number(*found, "processing.gpr_object_approach_min_frames");
|
||||
}
|
||||
if (const auto found = root.find("ignore_socket_speed"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.ignore_socket_speed must be bool");
|
||||
}
|
||||
config.ignore_socket_speed = found->get<bool>();
|
||||
if (const auto found = root.find("sar3d_frame_radius"); found != root.end()) {
|
||||
config.sar3d_frame_radius = parse_u32_number(*found, "processing.sar3d_frame_radius");
|
||||
}
|
||||
if (const auto found = root.find("reprocess_current_result"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.reprocess_current_result must be bool");
|
||||
}
|
||||
config.reprocess_current_result = found->get<bool>();
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"sar3d_voxel_x_m", &config.sar3d_voxel_x_m},
|
||||
std::pair{"sar3d_voxel_y_m", &config.sar3d_voxel_y_m},
|
||||
std::pair{"sar3d_voxel_z_m", &config.sar3d_voxel_z_m},
|
||||
std::pair{"sar3d_bp_ground_depth_m", &config.sar3d_bp_ground_depth_m},
|
||||
std::pair{"sar3d_comp_range_power", &config.sar3d_comp_range_power},
|
||||
std::pair{"sar3d_x_size_m", &config.sar3d_x_size_m},
|
||||
std::pair{"sar3d_y_min_m", &config.sar3d_y_min_m},
|
||||
std::pair{"sar3d_y_max_m", &config.sar3d_y_max_m},
|
||||
std::pair{"sar3d_z_size_m", &config.sar3d_z_size_m},
|
||||
}) {
|
||||
assign_optional(root, key, *target);
|
||||
}
|
||||
if (const auto found = root.find("history_command_seq"); found != root.end()) {
|
||||
config.history_command_seq = parse_u64_number(*found, "processing.history_command_seq");
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"sar3d_use_compensation", &config.sar3d_use_compensation},
|
||||
std::pair{"sar3d_normalize_each_frame", &config.sar3d_normalize_each_frame},
|
||||
std::pair{"sar3d_bg_gate_enabled", &config.sar3d_bg_gate_enabled},
|
||||
std::pair{"sar3d_clean_enabled", &config.sar3d_clean_enabled},
|
||||
}) {
|
||||
assign_optional(root, key, *target);
|
||||
}
|
||||
if (const auto found = root.find("history_command"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.history_command must be string");
|
||||
if (config.sar3d_x_size_m < 0.0F || config.sar3d_z_size_m < 0.0F ||
|
||||
config.sar3d_y_max_m <= config.sar3d_y_min_m) {
|
||||
throw std::runtime_error("SAR scene sizes must be non-negative and SAR_Y_RANGE_M must increase");
|
||||
}
|
||||
assign_optional(root, "ignore_socket_speed", config.ignore_socket_speed);
|
||||
assign_optional(root, "reprocess_current_result", config.reprocess_current_result);
|
||||
if (const auto found = root.find("history_clear_before_ns"); found != root.end()) {
|
||||
config.history_clear_before_ns = parse_u64_number(*found, "processing.history_clear_before_ns");
|
||||
}
|
||||
if (const auto found = root.find("history_removed"); found != root.end()) {
|
||||
if (!found->is_array()) {
|
||||
throw std::runtime_error("processing.history_removed must be an array");
|
||||
}
|
||||
for (const auto& value : *found) {
|
||||
HistoryTarget target{
|
||||
parse_u64_number(value.at("collection_id"), "history_removed.collection_id"),
|
||||
parse_u64_number(value.at("capture_start_ns"), "history_removed.capture_start_ns"),
|
||||
};
|
||||
if (!target.capture_start_ns) {
|
||||
throw std::runtime_error("history_removed.capture_start_ns must be positive");
|
||||
}
|
||||
config.history_removed.push_back(target);
|
||||
}
|
||||
config.history_command = parse_history_command(found->get<std::string>());
|
||||
}
|
||||
return config;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
ProcessingLiveConfigLoader::ProcessingLiveConfigLoader(std::string path)
|
||||
ProcessingLiveConfigLoader::ProcessingLiveConfigLoader(std::string path, const config::RunConfig& run_config)
|
||||
: path_(std::move(path)),
|
||||
run_config_(run_config),
|
||||
next_check_at_(std::chrono::steady_clock::now()) {
|
||||
if (path_.empty()) {
|
||||
throw std::runtime_error("Processing live config path must not be empty");
|
||||
@@ -446,9 +315,10 @@ auto ProcessingLiveConfigLoader::refresh_if_needed() -> ProcessingLiveConfig {
|
||||
return current_;
|
||||
}
|
||||
|
||||
current_ = parse_live_config(json_text, path_);
|
||||
// Remember rejected content too; retry only when the file actually changes.
|
||||
last_json_text_ = json_text;
|
||||
has_last_json_text_ = true;
|
||||
current_ = parse_live_config(json_text, path_, run_config_);
|
||||
++revision_;
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << "data_processor warning: failed to refresh live processing config: " << error.what() << '\n';
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -10,47 +9,9 @@
|
||||
|
||||
namespace radar::locator {
|
||||
|
||||
// Inclusive bounds used to clip locator observations to a visible window.
|
||||
struct VisibleBounds {
|
||||
float x_min = 0.0F;
|
||||
float x_max = 0.0F;
|
||||
float z_min = 0.0F;
|
||||
float z_max = 0.0F;
|
||||
};
|
||||
|
||||
// Limits mirroring the GUI semantics: if the number of detected objects exceeds
|
||||
// `max_detected_objects`, the result is intentionally empty (matches Python
|
||||
// reference). Otherwise, at most `draw_top_objects` rows are emitted.
|
||||
struct DrawLimits {
|
||||
std::uint32_t max_detected_objects = 0;
|
||||
std::uint32_t draw_top_objects = 0;
|
||||
};
|
||||
|
||||
// Filter parameters for the payload builder. None of these fields couple to GUI
|
||||
// state; they are resolved by the data_processor from the live processing config
|
||||
// and passed in explicitly so the builder remains a pure function.
|
||||
struct FilterParams {
|
||||
float min_score = 0.0F;
|
||||
std::optional<VisibleBounds> visible_bounds{};
|
||||
std::optional<DrawLimits> draw_limits{};
|
||||
};
|
||||
|
||||
// Extract observations from one result collection.
|
||||
//
|
||||
// Mirrors `python_app/orchestration/gpr_locator.py::locator_observations_from_collection`:
|
||||
// * Looks for a `gpr_points` TableF32 payload first, then falls back to
|
||||
// `gpr_region_centers`. Both layouts encode `[x_m, z_m, score, ...]` rows.
|
||||
// * Drops rows with non-finite coordinates and those below `min_score`.
|
||||
// * If `visible_bounds` are provided, drops rows outside the inclusive window.
|
||||
// * If `draw_limits` are provided and the surviving count exceeds
|
||||
// `max_detected_objects`, returns an empty vector (the GUI's "too many to
|
||||
// trust" heuristic). Otherwise, keeps the first `draw_top_objects` rows.
|
||||
//
|
||||
// The function never throws; malformed payloads degrade to an empty result.
|
||||
[[nodiscard]] auto observations_from_collection(
|
||||
const ipc::ResultCollection& collection,
|
||||
const FilterParams& filter
|
||||
) -> std::vector<Observation>;
|
||||
// Extract the final coherent-GPR objects, dropping malformed or non-finite rows.
|
||||
[[nodiscard]] auto observations_from_collection(const ipc::ResultCollection& collection)
|
||||
-> std::vector<Observation>;
|
||||
|
||||
// Serialize a list of observations into the JSON payload format expected by
|
||||
// locator clients: `{"ver": <n>, "tim": "HH:MM:SS.mmm", "sts": 1, "obs": [...]}`.
|
||||
|
||||
@@ -36,7 +36,6 @@ class ClientQueue {
|
||||
// Wake any waiter and reject further pushes. Idempotent.
|
||||
void close();
|
||||
|
||||
[[nodiscard]] auto is_closed() const -> bool;
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_{};
|
||||
@@ -88,6 +87,7 @@ class ClientSession {
|
||||
private:
|
||||
void writer_loop();
|
||||
void reader_loop(std::atomic<double>& shared_vlc_slot, std::atomic<std::int64_t>& shared_vlc_at_ns);
|
||||
void finish_loop();
|
||||
|
||||
int socket_fd_;
|
||||
std::string peer_name_;
|
||||
@@ -138,7 +138,7 @@ class TcpServer {
|
||||
// Build and broadcast one locator packet derived from a GPR result
|
||||
// collection. Never blocks for I/O. Filter parameters are supplied by the
|
||||
// caller so the builder remains state-free.
|
||||
void publish(const ipc::ResultCollection& collection, const FilterParams& filter);
|
||||
void publish(const ipc::ResultCollection& collection);
|
||||
|
||||
// Most recent `vlc` value received from any connected client, or nullopt
|
||||
// if no client has ever submitted one since startup. Lock-free.
|
||||
@@ -148,11 +148,7 @@ class TcpServer {
|
||||
void acceptor_loop();
|
||||
void enroll_client(std::unique_ptr<ClientSession> session);
|
||||
void broadcast_packet(const std::vector<std::uint8_t>& packet);
|
||||
void reap_finished_clients();
|
||||
// Best-effort reap from the publish path: acquires clients_mutex_ via try-lock
|
||||
// (skips if contended) and joins exited sessions outside the lock. Keeps
|
||||
// clients_ bounded even when no new connections arrive to trigger a reap.
|
||||
void try_reap_finished_clients();
|
||||
void reap_finished_clients(bool wait_for_lock = true);
|
||||
void cache_latest_packet(std::vector<std::uint8_t> packet);
|
||||
[[nodiscard]] auto latest_packet_copy() const -> std::optional<std::vector<std::uint8_t>>;
|
||||
|
||||
|
||||
@@ -6,7 +6,9 @@
|
||||
#include <cstring>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
@@ -17,14 +19,12 @@ namespace {
|
||||
using Json = nlohmann::json;
|
||||
|
||||
constexpr const char* kGprPointsName = "gpr_points";
|
||||
constexpr const char* kGprRegionCentersName = "gpr_region_centers";
|
||||
constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
|
||||
|
||||
// Locate the first TableF32 payload matching one of the GPR object names.
|
||||
// Returns nullptr if no usable payload is present in the collection.
|
||||
[[nodiscard]] auto find_object_table(const ipc::ResultCollection& collection)
|
||||
-> const ipc::ResultPayload* {
|
||||
const ipc::ResultPayload* fallback = nullptr;
|
||||
for (const auto& payload : collection.collection_payloads) {
|
||||
if (payload.kind != ipc::ResultKind::TableF32) {
|
||||
continue;
|
||||
@@ -35,11 +35,8 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
|
||||
if (payload.processing_name == kGprPointsName) {
|
||||
return &payload;
|
||||
}
|
||||
if (fallback == nullptr && payload.processing_name == kGprRegionCentersName) {
|
||||
fallback = &payload;
|
||||
}
|
||||
}
|
||||
return fallback;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Quantise to one decimal place (0.1 m = decimetres). Equivalent to Python's
|
||||
@@ -49,29 +46,6 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
|
||||
return std::round(value * 10.0) / 10.0;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto passes_basic_filter(
|
||||
float x_m,
|
||||
float z_m,
|
||||
float score,
|
||||
float min_score
|
||||
) -> bool {
|
||||
if (!std::isfinite(x_m) || !std::isfinite(z_m) || !std::isfinite(score)) {
|
||||
return false;
|
||||
}
|
||||
return score >= min_score;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto passes_visible_bounds(
|
||||
float x_m,
|
||||
float z_m,
|
||||
const VisibleBounds& bounds
|
||||
) -> bool {
|
||||
return x_m >= bounds.x_min
|
||||
&& x_m <= bounds.x_max
|
||||
&& z_m >= bounds.z_min
|
||||
&& z_m <= bounds.z_max;
|
||||
}
|
||||
|
||||
// Render the current wall-clock time as "HH:MM:SS.mmm". Uses localtime to match
|
||||
// the Python reference behaviour (which calls datetime.now() with no tz info).
|
||||
[[nodiscard]] auto format_timestamp_now() -> std::string {
|
||||
@@ -96,17 +70,6 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
// Wall-clock epoch milliseconds at packet-build time. Unlike the human-readable
|
||||
// `tim` field (which has no date and rolls over at midnight), this is a
|
||||
// monotonically increasing generation stamp consumers can use to compute the
|
||||
// true age of a snapshot and detect a stalled pipeline.
|
||||
[[nodiscard]] auto epoch_millis_now() -> std::int64_t {
|
||||
using Clock = std::chrono::system_clock;
|
||||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
Clock::now().time_since_epoch()
|
||||
).count();
|
||||
}
|
||||
|
||||
void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t value) {
|
||||
buffer.push_back(static_cast<std::uint8_t>(value & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 8U) & 0xFFU));
|
||||
@@ -116,66 +79,20 @@ void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t v
|
||||
|
||||
} // namespace
|
||||
|
||||
auto observations_from_collection(
|
||||
const ipc::ResultCollection& collection,
|
||||
const FilterParams& filter
|
||||
) -> std::vector<Observation> {
|
||||
auto observations_from_collection(const ipc::ResultCollection& collection) -> std::vector<Observation> {
|
||||
const auto* payload = find_object_table(collection);
|
||||
if (payload == nullptr) {
|
||||
return {};
|
||||
}
|
||||
|
||||
const std::uint32_t columns = payload->table_columns;
|
||||
if (columns == 0U) {
|
||||
return {};
|
||||
}
|
||||
const std::size_t row_count = payload->table_values.size() / columns;
|
||||
if (row_count == 0U) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<Observation> visible;
|
||||
visible.reserve(row_count);
|
||||
for (std::size_t row = 0; row < row_count; ++row) {
|
||||
const std::size_t base = row * static_cast<std::size_t>(columns);
|
||||
const float x_m = payload->table_values[base + 0U];
|
||||
const float z_m = payload->table_values[base + 1U];
|
||||
const float score = payload->table_values[base + 2U];
|
||||
|
||||
if (!passes_basic_filter(x_m, z_m, score, filter.min_score)) {
|
||||
continue;
|
||||
}
|
||||
if (filter.visible_bounds.has_value()
|
||||
&& !passes_visible_bounds(x_m, z_m, *filter.visible_bounds)) {
|
||||
continue;
|
||||
}
|
||||
visible.push_back({.crs = x_m, .dst = z_m});
|
||||
}
|
||||
|
||||
if (!filter.draw_limits.has_value()) {
|
||||
std::vector<Observation> quantised;
|
||||
quantised.reserve(visible.size());
|
||||
for (const auto& observation : visible) {
|
||||
quantised.push_back({
|
||||
.crs = quantise_to_decimetres(observation.crs),
|
||||
.dst = quantise_to_decimetres(observation.dst),
|
||||
});
|
||||
}
|
||||
return quantised;
|
||||
}
|
||||
|
||||
const auto& limits = *filter.draw_limits;
|
||||
if (visible.size() > limits.max_detected_objects) {
|
||||
return {};
|
||||
}
|
||||
const std::size_t kept = std::min<std::size_t>(visible.size(), limits.draw_top_objects);
|
||||
const std::size_t columns = payload->table_columns;
|
||||
std::vector<Observation> result;
|
||||
result.reserve(kept);
|
||||
for (std::size_t index = 0; index < kept; ++index) {
|
||||
result.push_back({
|
||||
.crs = quantise_to_decimetres(visible[index].crs),
|
||||
.dst = quantise_to_decimetres(visible[index].dst),
|
||||
});
|
||||
for (std::size_t base = 0; base + columns <= payload->table_values.size(); base += columns) {
|
||||
const auto x = payload->table_values[base];
|
||||
const auto z = payload->table_values[base + 1];
|
||||
const auto score = payload->table_values[base + 2];
|
||||
if (std::isfinite(x) && std::isfinite(z) && std::isfinite(score)) {
|
||||
result.push_back({.crs = quantise_to_decimetres(x), .dst = quantise_to_decimetres(z)});
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
@@ -200,15 +117,6 @@ auto build_payload_json(
|
||||
{"obs", std::move(obs_array)},
|
||||
};
|
||||
|
||||
// `gen` is a build-time generation stamp (epoch ms) that lets a consumer compute
|
||||
// a packet's true age and flag a cached/stalled snapshot — something `tim` cannot
|
||||
// express, having no date and rolling over at midnight. It is currently NOT sent;
|
||||
// set this flag to true to re-enable it (epoch_millis_now() is kept ready for that).
|
||||
constexpr bool kEmitGenerationStamp = false;
|
||||
if (kEmitGenerationStamp) {
|
||||
root["gen"] = epoch_millis_now();
|
||||
}
|
||||
|
||||
return root.dump();
|
||||
}
|
||||
|
||||
@@ -216,6 +124,9 @@ auto encode_packet(
|
||||
const std::string& payload_json,
|
||||
std::uint32_t device_id
|
||||
) -> std::vector<std::uint8_t> {
|
||||
if (payload_json.size() > std::numeric_limits<std::uint32_t>::max()) {
|
||||
throw std::runtime_error("Locator packet payload exceeds uint32 wire limit");
|
||||
}
|
||||
const auto payload_size = static_cast<std::uint32_t>(payload_json.size());
|
||||
std::vector<std::uint8_t> packet;
|
||||
packet.reserve(static_cast<std::size_t>(8U) + payload_json.size());
|
||||
|
||||
@@ -197,11 +197,6 @@ void ClientQueue::close() {
|
||||
not_empty_.notify_all();
|
||||
}
|
||||
|
||||
auto ClientQueue::is_closed() const -> bool {
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
return closed_;
|
||||
}
|
||||
|
||||
// ----- ClientSession --------------------------------------------------------
|
||||
|
||||
ClientSession::ClientSession(
|
||||
@@ -225,10 +220,16 @@ void ClientSession::start(
|
||||
std::atomic<double>& shared_vlc_slot,
|
||||
std::atomic<std::int64_t>& shared_vlc_at_ns
|
||||
) {
|
||||
writer_thread_ = std::thread([this]() { writer_loop(); });
|
||||
reader_thread_ = std::thread([this, &shared_vlc_slot, &shared_vlc_at_ns]() {
|
||||
reader_loop(shared_vlc_slot, shared_vlc_at_ns);
|
||||
});
|
||||
try {
|
||||
writer_thread_ = std::thread([this]() { writer_loop(); });
|
||||
reader_thread_ = std::thread([this, &shared_vlc_slot, &shared_vlc_at_ns]() {
|
||||
reader_loop(shared_vlc_slot, shared_vlc_at_ns);
|
||||
});
|
||||
} catch (...) {
|
||||
request_stop();
|
||||
join();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void ClientSession::enqueue(std::vector<std::uint8_t> packet) {
|
||||
@@ -278,14 +279,7 @@ void ClientSession::writer_loop() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
request_stop();
|
||||
// Mark exited only once BOTH loops have finished; otherwise a writer-only
|
||||
// death (e.g. a send timeout while the reader still blocks on recv) would
|
||||
// never be reaped. request_stop() above shuts the socket so the reader's
|
||||
// recv() returns promptly.
|
||||
if (loops_finished_.fetch_add(1, std::memory_order_acq_rel) + 1U == 2U) {
|
||||
exited_.store(true, std::memory_order_release);
|
||||
}
|
||||
finish_loop();
|
||||
}
|
||||
|
||||
void ClientSession::reader_loop(
|
||||
@@ -295,51 +289,58 @@ void ClientSession::reader_loop(
|
||||
std::array<std::uint8_t, kPacketHeaderSize> header_buffer{};
|
||||
std::vector<std::uint8_t> payload_buffer;
|
||||
|
||||
while (!stop_requested_.load(std::memory_order_acquire)) {
|
||||
if (!read_exact(socket_fd_, header_buffer.data(), header_buffer.size(), stop_requested_)) {
|
||||
break;
|
||||
}
|
||||
const auto payload_len = decode_u32_little_endian(header_buffer.data() + 4U);
|
||||
if (payload_len > max_payload_bytes_) {
|
||||
log_warning(
|
||||
"closing client " + peer_name_ + " after payload size "
|
||||
+ std::to_string(payload_len) + " exceeded the configured limit"
|
||||
);
|
||||
break;
|
||||
}
|
||||
try {
|
||||
while (!stop_requested_.load(std::memory_order_acquire)) {
|
||||
if (!read_exact(socket_fd_, header_buffer.data(), header_buffer.size(), stop_requested_)) {
|
||||
break;
|
||||
}
|
||||
const auto payload_len = decode_u32_little_endian(header_buffer.data() + 4U);
|
||||
if (payload_len > max_payload_bytes_) {
|
||||
log_warning(
|
||||
"closing client " + peer_name_ + " after payload size "
|
||||
+ std::to_string(payload_len) + " exceeded the configured limit"
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
payload_buffer.assign(payload_len, std::uint8_t{0});
|
||||
if (payload_len > 0U
|
||||
&& !read_exact(socket_fd_, payload_buffer.data(), payload_len, stop_requested_)) {
|
||||
break;
|
||||
}
|
||||
payload_buffer.assign(payload_len, std::uint8_t{0});
|
||||
if (payload_len > 0U
|
||||
&& !read_exact(socket_fd_, payload_buffer.data(), payload_len, stop_requested_)) {
|
||||
break;
|
||||
}
|
||||
|
||||
try {
|
||||
const auto json = Json::parse(payload_buffer.begin(), payload_buffer.end());
|
||||
if (json.is_object()) {
|
||||
const auto found = json.find("vlc");
|
||||
if (found != json.end() && found->is_number()) {
|
||||
const double value = found->get<double>();
|
||||
if (std::isfinite(value)) {
|
||||
// Stamp the value first, then the time, so a reader that
|
||||
// observes a fresh timestamp also observes the matching value.
|
||||
shared_vlc_slot.store(value, std::memory_order_release);
|
||||
const auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()
|
||||
).count();
|
||||
shared_vlc_at_ns.store(now_ns, std::memory_order_release);
|
||||
try {
|
||||
const auto json = Json::parse(payload_buffer.begin(), payload_buffer.end());
|
||||
if (json.is_object()) {
|
||||
const auto found = json.find("vlc");
|
||||
if (found != json.end() && found->is_number()) {
|
||||
const double value = found->get<double>();
|
||||
if (std::isfinite(value) && std::fabs(value) <= std::numeric_limits<float>::max()) {
|
||||
// Stamp the value first, then the time, so a reader that
|
||||
// observes a fresh timestamp also observes the matching value.
|
||||
shared_vlc_slot.store(value, std::memory_order_release);
|
||||
const auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()
|
||||
).count();
|
||||
shared_vlc_at_ns.store(now_ns, std::memory_order_release);
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (const Json::exception& error) {
|
||||
log_warning(
|
||||
"ignoring malformed packet from " + peer_name_ + ": " + error.what()
|
||||
);
|
||||
}
|
||||
} catch (const Json::parse_error& error) {
|
||||
log_warning(
|
||||
"ignoring malformed packet from " + peer_name_ + ": " + error.what()
|
||||
);
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
log_warning("closing client " + peer_name_ + ": " + error.what());
|
||||
}
|
||||
finish_loop();
|
||||
}
|
||||
|
||||
void ClientSession::finish_loop() {
|
||||
request_stop();
|
||||
// Mark exited only once BOTH loops have finished (see writer_loop), so a
|
||||
// reader-only death still waits for the writer before this session is reaped.
|
||||
// A session is reapable after both threads have stopped using its socket.
|
||||
if (loops_finished_.fetch_add(1, std::memory_order_acq_rel) + 1U == 2U) {
|
||||
exited_.store(true, std::memory_order_release);
|
||||
}
|
||||
@@ -417,7 +418,13 @@ void TcpServer::start() {
|
||||
}
|
||||
|
||||
listen_fd_ = fd;
|
||||
acceptor_thread_ = std::thread([this]() { acceptor_loop(); });
|
||||
try {
|
||||
acceptor_thread_ = std::thread([this]() { acceptor_loop(); });
|
||||
} catch (...) {
|
||||
shutdown_and_close(listen_fd_);
|
||||
running_.store(false, std::memory_order_release);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::stop() {
|
||||
@@ -427,13 +434,14 @@ void TcpServer::stop() {
|
||||
|
||||
if (listen_fd_ >= 0) {
|
||||
(void)::shutdown(listen_fd_, SHUT_RDWR);
|
||||
(void)::close(listen_fd_);
|
||||
listen_fd_ = -1;
|
||||
}
|
||||
|
||||
if (acceptor_thread_.joinable()) {
|
||||
acceptor_thread_.join();
|
||||
}
|
||||
// Keep the descriptor stable until accept() has returned: closing it earlier
|
||||
// permits fd reuse and races the acceptor's read of listen_fd_.
|
||||
shutdown_and_close(listen_fd_);
|
||||
|
||||
std::vector<std::unique_ptr<ClientSession>> sessions;
|
||||
{
|
||||
@@ -453,12 +461,12 @@ auto TcpServer::is_running() const -> bool {
|
||||
return running_.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
void TcpServer::publish(const ipc::ResultCollection& collection, const FilterParams& filter) {
|
||||
void TcpServer::publish(const ipc::ResultCollection& collection) {
|
||||
if (!running_.load(std::memory_order_acquire)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto observations = observations_from_collection(collection, filter);
|
||||
const auto observations = observations_from_collection(collection);
|
||||
const auto payload_json = build_payload_json(observations, config_.protocol_version);
|
||||
auto packet = encode_packet(payload_json, config_.device_id);
|
||||
cache_latest_packet(packet);
|
||||
@@ -466,7 +474,7 @@ void TcpServer::publish(const ipc::ResultCollection& collection, const FilterPar
|
||||
// Reap exited sessions on the publish path too, so clients_ stays bounded
|
||||
// even when no new connections arrive to trigger an accept-time reap. Uses a
|
||||
// try-lock and joins outside the mutex, so publish() never blocks on it.
|
||||
try_reap_finished_clients();
|
||||
reap_finished_clients(false);
|
||||
}
|
||||
|
||||
auto TcpServer::latest_socket_speed() const -> std::optional<double> {
|
||||
@@ -525,19 +533,27 @@ void TcpServer::acceptor_loop() {
|
||||
|
||||
reap_finished_clients();
|
||||
|
||||
apply_socket_keepalive(client_fd);
|
||||
auto session = std::make_unique<ClientSession>(
|
||||
client_fd,
|
||||
format_peer(peer_addr),
|
||||
config_.client_queue_size,
|
||||
config_.max_payload_bytes
|
||||
);
|
||||
const auto snapshot = latest_packet_copy();
|
||||
if (snapshot.has_value()) {
|
||||
session->enqueue(*snapshot);
|
||||
std::unique_ptr<ClientSession> session;
|
||||
bool socket_owned = false;
|
||||
try {
|
||||
apply_socket_keepalive(client_fd);
|
||||
session = std::make_unique<ClientSession>(
|
||||
client_fd, format_peer(peer_addr), config_.client_queue_size,
|
||||
config_.max_payload_bytes
|
||||
);
|
||||
socket_owned = true;
|
||||
const auto snapshot = latest_packet_copy();
|
||||
if (snapshot.has_value()) {
|
||||
session->enqueue(*snapshot);
|
||||
}
|
||||
session->start(latest_socket_speed_, latest_socket_speed_at_ns_);
|
||||
enroll_client(std::move(session));
|
||||
} catch (const std::exception& error) {
|
||||
if (!socket_owned) {
|
||||
::close(client_fd);
|
||||
}
|
||||
log_warning(std::string("cannot start client: ") + error.what());
|
||||
}
|
||||
session->start(latest_socket_speed_, latest_socket_speed_at_ns_);
|
||||
enroll_client(std::move(session));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -553,35 +569,13 @@ void TcpServer::broadcast_packet(const std::vector<std::uint8_t>& packet) {
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::reap_finished_clients() {
|
||||
void TcpServer::reap_finished_clients(bool wait_for_lock) {
|
||||
std::vector<std::unique_ptr<ClientSession>> to_join;
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(clients_mutex_);
|
||||
auto first_dead = std::partition(
|
||||
clients_.begin(),
|
||||
clients_.end(),
|
||||
[](const std::unique_ptr<ClientSession>& session) {
|
||||
return !session->has_exited();
|
||||
}
|
||||
);
|
||||
for (auto it = first_dead; it != clients_.end(); ++it) {
|
||||
to_join.push_back(std::move(*it));
|
||||
}
|
||||
clients_.erase(first_dead, clients_.end());
|
||||
}
|
||||
for (auto& session : to_join) {
|
||||
session->request_stop();
|
||||
session->join();
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::try_reap_finished_clients() {
|
||||
std::vector<std::unique_ptr<ClientSession>> to_join;
|
||||
{
|
||||
// try_to_lock: if the acceptor already holds the mutex (it reaps too),
|
||||
// skip this round rather than block the publish path.
|
||||
std::unique_lock<std::mutex> guard(clients_mutex_, std::try_to_lock);
|
||||
if (!guard.owns_lock()) {
|
||||
std::unique_lock<std::mutex> guard(clients_mutex_, std::defer_lock);
|
||||
if (wait_for_lock) {
|
||||
guard.lock();
|
||||
} else if (!guard.try_lock()) {
|
||||
return;
|
||||
}
|
||||
auto first_dead = std::partition(
|
||||
|
||||
@@ -21,15 +21,4 @@ class GprProcessor final : public ProcessorInterface {
|
||||
ObjectApproachFilter approach_filter_{};
|
||||
};
|
||||
|
||||
class LegacyGprProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
[[nodiscard]] auto name() const -> std::string override;
|
||||
[[nodiscard]] auto process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection override;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
|
||||
@@ -10,8 +10,7 @@
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
// Temporal "approach" filter for coherent-BP detections — a port of the objects-only
|
||||
// filter in Horns_motion_3libre's demonstrate notebook.
|
||||
// Temporal "approach" filter for coherent-BP detections.
|
||||
//
|
||||
// It keeps only objects that persist as a *motion-consistent track* across at least
|
||||
// `min_frames` consecutive frames: as the radar moves, a real target reappears at a
|
||||
@@ -30,12 +29,8 @@ namespace radar::processing {
|
||||
// inter-frame gap (`kMaxFrameGapSeconds`), so a stale history from a previous run cannot
|
||||
// spuriously confirm objects.
|
||||
//
|
||||
// IDEMPOTENT under reprocessing: the data_processor re-runs the last collection (or
|
||||
// replays the whole window) whenever live settings or the socket speed change, with no
|
||||
// new sweep. The history is therefore keyed by `frame_id` (the strictly increasing
|
||||
// collection id): the same id replaces its entry (reprocess of the current frame), a
|
||||
// smaller id rebuilds from scratch (a replay restart), so repeated reprocessing never
|
||||
// duplicates frames or falsely confirms a track.
|
||||
// Reprocessing replaces the same (collection_id, capture_start_ns) entry. Earlier
|
||||
// capture times rebuild the history when the processor replays its retained window.
|
||||
class ObjectApproachFilter {
|
||||
public:
|
||||
struct Point {
|
||||
@@ -43,20 +38,16 @@ class ObjectApproachFilter {
|
||||
double z_m;
|
||||
};
|
||||
|
||||
// Record `objects` as frame `frame_id` and return, per object, whether it is
|
||||
// confirmed (ends a >= `min_frames` motion-consistent track). `frame_id` is the
|
||||
// collection id (identity/order, survives reprocessing); `frame_time_seconds` is the
|
||||
// frame's wall-clock timestamp (drives the inter-frame interval); `speed_m_s`/
|
||||
// `look_angle_deg` are the live motion estimate. `min_frames <= 1` disables filtering.
|
||||
// Acquisition time drives both ordering and motion intervals. min_frames <= 1 disables filtering.
|
||||
[[nodiscard]] auto confirm(
|
||||
const std::vector<Point>& objects,
|
||||
std::uint64_t frame_id,
|
||||
double frame_time_seconds,
|
||||
std::uint64_t capture_start_ns,
|
||||
double speed_m_s,
|
||||
double look_angle_deg,
|
||||
std::size_t min_frames
|
||||
) -> std::vector<bool> {
|
||||
record_frame(Frame{frame_id, frame_time_seconds, objects});
|
||||
record_frame(Frame{frame_id, capture_start_ns, objects});
|
||||
const std::size_t history_depth = std::max<std::size_t>(min_frames, 1U);
|
||||
while (history_.size() > history_depth) {
|
||||
history_.pop_front();
|
||||
@@ -79,17 +70,14 @@ class ObjectApproachFilter {
|
||||
private:
|
||||
struct Frame {
|
||||
std::uint64_t id;
|
||||
double time_seconds;
|
||||
std::uint64_t capture_start_ns;
|
||||
std::vector<Point> objects;
|
||||
};
|
||||
|
||||
// Append a genuinely new frame, replace the current one on reprocessing (same id),
|
||||
// or rebuild from scratch when the id steps backward (a replay restart). This keeps
|
||||
// the history one entry per distinct collection no matter how often settings change.
|
||||
void record_frame(Frame frame) {
|
||||
if (history_.empty() || frame.id > history_.back().id) {
|
||||
if (history_.empty() || frame.capture_start_ns > history_.back().capture_start_ns) {
|
||||
history_.push_back(std::move(frame));
|
||||
} else if (frame.id == history_.back().id) {
|
||||
} else if (frame.id == history_.back().id && frame.capture_start_ns == history_.back().capture_start_ns) {
|
||||
history_.back() = std::move(frame);
|
||||
} else {
|
||||
history_.clear();
|
||||
@@ -97,7 +85,7 @@ class ObjectApproachFilter {
|
||||
}
|
||||
}
|
||||
|
||||
// Fixed matching tolerances (Horns_motion notebook 0.2 block). Range decreases as the
|
||||
// Fixed matching tolerances. Range decreases as the
|
||||
// radar approaches the target, hence the negative Z sign.
|
||||
static constexpr double kXToleranceM = 0.45;
|
||||
static constexpr double kRangeToleranceFraction = 0.85;
|
||||
@@ -122,8 +110,10 @@ class ObjectApproachFilter {
|
||||
for (std::size_t step = 1U; step < min_frames; ++step) {
|
||||
const std::size_t earlier_index = newest - step;
|
||||
const Frame& earlier = history_[earlier_index];
|
||||
const double dt = history_[earlier_index + 1U].time_seconds - earlier.time_seconds;
|
||||
if (!(dt > 0.0) || dt > kMaxFrameGapSeconds) {
|
||||
const auto later_ns = history_[earlier_index + 1U].capture_start_ns;
|
||||
if (later_ns <= earlier.capture_start_ns) { return false; }
|
||||
const double dt = static_cast<double>(later_ns - earlier.capture_start_ns) * 1e-9;
|
||||
if (dt > kMaxFrameGapSeconds) {
|
||||
return false; // non-monotonic time, or a gap that breaks the track
|
||||
}
|
||||
const double expected_range_shift = range_step_per_second * dt;
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <string>
|
||||
|
||||
#include "processor_interface.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
// 3D SAR of the reference notebook Multi_depthnorm_clean.ipynb on an NVIDIA GPU.
|
||||
// Publishes max-projections of the normalized volume, or of the CLEAN display, as
|
||||
// ImageF32 payloads: sar3d_front (X-Y), sar3d_side (Z-Y), sar3d_top (X-Z) and
|
||||
// sar3d_oblique (the notebook's 3D scene view, axes in the plane of
|
||||
// sar3d_oblique_basis); sar3d_peak = [level, X, Y, Z] of the shown peak; the frame's
|
||||
// sar3d_background ratio and the CLEAN sar3d_objects and sar3d_marks tables (see
|
||||
// python_app/gui/controllers/renderers/sar3d.py).
|
||||
class Sar3dProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
Sar3dProcessor();
|
||||
~Sar3dProcessor() override;
|
||||
|
||||
[[nodiscard]] auto name() const -> std::string override;
|
||||
[[nodiscard]] auto process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection override;
|
||||
|
||||
private:
|
||||
struct State;
|
||||
std::unique_ptr<State> state_;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,248 @@
|
||||
#pragma once
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include <map>
|
||||
#include <limits>
|
||||
#include <numbers>
|
||||
#include <stdexcept>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::processing::bp {
|
||||
|
||||
constexpr double kWaveSpeed = 3e8; // v in both reference algorithms
|
||||
using Background = std::unordered_map<ipc::ComboKey, std::vector<std::complex<double>>, ipc::ComboKeyHash>;
|
||||
|
||||
// numpy.median: the middle value, or the mean of the middle two for an even count.
|
||||
template <class Value>
|
||||
[[nodiscard]] auto median(std::vector<Value> values) -> double {
|
||||
if (values.empty()) { throw std::logic_error("median of no values"); }
|
||||
const auto middle = values.begin() + static_cast<std::ptrdiff_t>(values.size() / 2U);
|
||||
std::nth_element(values.begin(), middle, values.end());
|
||||
if (values.size() % 2U == 1U) { return static_cast<double>(*middle); }
|
||||
return 0.5 * (static_cast<double>(*middle) + static_cast<double>(*std::max_element(values.begin(), middle)));
|
||||
}
|
||||
|
||||
// Mean of up to count previous collections, separately for each physical pair.
|
||||
inline auto background_mean(std::span<const ipc::PreprocessedCollection> previous, bool enabled,
|
||||
std::size_t count) -> Background {
|
||||
Background result;
|
||||
if (!enabled || count == 0U) { return result; }
|
||||
std::unordered_map<ipc::ComboKey, std::size_t, ipc::ComboKeyHash> counts;
|
||||
for (const auto& collection : previous.last(std::min(count, previous.size()))) {
|
||||
for (const auto& trace : collection.traces) {
|
||||
const auto key = trace.combo;
|
||||
auto& sum = result[key];
|
||||
if (sum.empty()) { sum.resize(trace.s21.size()); }
|
||||
if (sum.size() != trace.s21.size()) { continue; }
|
||||
for (std::size_t i = 0; i < sum.size(); ++i) {
|
||||
sum[i] += std::complex<double>(trace.s21[i].re, trace.s21[i].im);
|
||||
}
|
||||
++counts[key];
|
||||
}
|
||||
}
|
||||
for (auto& [key, samples] : result) {
|
||||
for (auto& sample : samples) { sample /= static_cast<double>(counts.at(key)); }
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
inline auto subtract_background(const ipc::SweepTraceBlock& trace, const Background& background)
|
||||
-> std::vector<std::complex<double>> {
|
||||
std::vector<std::complex<double>> samples;
|
||||
samples.reserve(trace.s21.size());
|
||||
const auto found = background.find(trace.combo);
|
||||
const bool subtract = found != background.end() && found->second.size() == trace.s21.size();
|
||||
for (std::size_t i = 0; i < trace.s21.size(); ++i) {
|
||||
samples.emplace_back(std::complex<double>(trace.s21[i].re, trace.s21[i].im) -
|
||||
(subtract ? found->second[i] : std::complex<double>{}));
|
||||
}
|
||||
return samples;
|
||||
}
|
||||
|
||||
// When the samples of a trace were measured, relative to the acquisition centre
|
||||
// of its collection (compute_pair_timestamps_for_snapshot and
|
||||
// compute_frequency_sample_times). Traces sharing a capture window come from
|
||||
// one sweep: at every frequency point its stimulus stages - the outputs, which
|
||||
// are numbered in device stage order - take turns while all inputs receive.
|
||||
struct TraceClock {
|
||||
double start_s = 0.0; // sweep start
|
||||
double event_s = 0.0; // sweep duration / (stages * points)
|
||||
std::size_t stage = 0U;
|
||||
std::size_t stages = 1U;
|
||||
|
||||
[[nodiscard]] auto sample_time_s(std::size_t point) const -> double {
|
||||
return start_s + (static_cast<double>(stages) * point + stage) * event_s;
|
||||
}
|
||||
};
|
||||
|
||||
struct CollectionTiming {
|
||||
std::uint64_t center_ns = 0U; // centre of the collection's capture window
|
||||
std::vector<TraceClock> clocks; // per collection trace
|
||||
};
|
||||
|
||||
// Timing from the per-trace capture windows (snapshot_acquisition_timing); every
|
||||
// trace takes part, so the stages do not depend on the INPUT_IDX/OUTPUT_IDX choice.
|
||||
[[nodiscard]] inline auto collection_timing(const ipc::PreprocessedCollection& collection) -> CollectionTiming {
|
||||
if (collection.traces.empty()) {
|
||||
throw std::runtime_error("collection has no traces");
|
||||
}
|
||||
std::map<std::pair<std::uint64_t, std::uint64_t>, std::vector<std::uint32_t>> sweep_outputs{};
|
||||
std::uint64_t first = std::numeric_limits<std::uint64_t>::max();
|
||||
std::uint64_t last = 0U;
|
||||
for (const auto& trace : collection.traces) {
|
||||
if (trace.capture_end_ns <= trace.capture_start_ns) {
|
||||
throw std::runtime_error("collection has traces without a capture window");
|
||||
}
|
||||
if (trace.frequency_hz.empty()) {
|
||||
throw std::runtime_error("collection has a trace without frequency points");
|
||||
}
|
||||
sweep_outputs[{trace.capture_start_ns, trace.capture_end_ns}].push_back(trace.combo.output_pos);
|
||||
first = std::min(first, trace.capture_start_ns);
|
||||
last = std::max(last, trace.capture_end_ns);
|
||||
}
|
||||
for (auto& [window, outputs] : sweep_outputs) {
|
||||
std::sort(outputs.begin(), outputs.end());
|
||||
outputs.erase(std::unique(outputs.begin(), outputs.end()), outputs.end());
|
||||
}
|
||||
|
||||
CollectionTiming timing{};
|
||||
const auto duration_ns = last - first;
|
||||
timing.center_ns = first + duration_ns / 2U;
|
||||
for (const auto& trace : collection.traces) {
|
||||
const auto& stages = sweep_outputs.at({trace.capture_start_ns, trace.capture_end_ns});
|
||||
const auto stage = std::find(stages.begin(), stages.end(), trace.combo.output_pos) - stages.begin();
|
||||
TraceClock clock{};
|
||||
clock.start_s = 1e-9 * (static_cast<double>(trace.capture_start_ns - first) -
|
||||
0.5 * static_cast<double>(duration_ns));
|
||||
clock.stage = stage;
|
||||
clock.stages = stages.size();
|
||||
clock.event_s = 1e-9 * static_cast<double>(trace.capture_end_ns - trace.capture_start_ns) /
|
||||
static_cast<double>(clock.stages) / trace.frequency_hz.size();
|
||||
timing.clocks.push_back(clock);
|
||||
}
|
||||
return timing;
|
||||
}
|
||||
|
||||
// apply_intra_sweep_phase_correction: one phase model includes the pair offset
|
||||
// and the intra-sweep motion. Focus mode removes its constant and linear terms.
|
||||
inline void apply_motion_phase(std::vector<std::complex<double>>& samples, const std::vector<float>& frequency,
|
||||
const TraceClock& clock, double speed, double pitch, double direction, bool focus,
|
||||
double f_start, double f_stop) {
|
||||
if (speed == 0.0) { return; }
|
||||
std::vector<std::size_t> points;
|
||||
std::vector<double> phase;
|
||||
const double factor = 4.0 * std::numbers::pi * direction * speed * std::cos(pitch) / kWaveSpeed;
|
||||
double sum_f = 0.0, sum_phase = 0.0;
|
||||
for (std::size_t i = 0; i < frequency.size(); ++i) {
|
||||
if (frequency[i] < f_start || frequency[i] > f_stop) { continue; }
|
||||
points.push_back(i);
|
||||
phase.push_back(factor * frequency[i] * clock.sample_time_s(i));
|
||||
sum_f += frequency[i];
|
||||
sum_phase += phase.back();
|
||||
}
|
||||
if (points.empty()) { return; }
|
||||
if (focus) {
|
||||
const double mean_f = sum_f / points.size(), mean_phase = sum_phase / points.size();
|
||||
double cross = 0.0, square = 0.0;
|
||||
for (std::size_t i = 0; i < points.size(); ++i) {
|
||||
const double f = frequency[points[i]] - mean_f;
|
||||
cross += f * phase[i]; square += f * f;
|
||||
}
|
||||
const double slope = square > 0.0 ? cross / square : 0.0;
|
||||
for (std::size_t i = 0; i < points.size(); ++i) {
|
||||
phase[i] -= mean_phase + slope * (frequency[points[i]] - mean_f);
|
||||
}
|
||||
}
|
||||
for (std::size_t i = 0; i < points.size(); ++i) { samples[points[i]] *= std::polar(1.0, -phase[i]); }
|
||||
}
|
||||
|
||||
// pair_depth_eq_z_max: cover the far scene corner, aperture and half a median window.
|
||||
inline auto equalize_depth_limit(double max_depth, double x_min, double x_max, double antenna_x_max) -> double {
|
||||
const double x_far = std::max(std::abs(x_min), std::abs(x_max)) + antenna_x_max;
|
||||
return std::sqrt(max_depth * max_depth + x_far * x_far + 2.5 * 2.5) + 1.5 + 0.75;
|
||||
}
|
||||
|
||||
// Radar axes (Y, Z) of a scene point at height y above the ground and z along it, for a radar
|
||||
// pitched by `pitch` with its origin `height` above the ground (sar_scene_to_radar_yz).
|
||||
inline auto radar_yz(double y, double z, double height, double pitch) -> std::array<double, 2> {
|
||||
const double y0 = y - height;
|
||||
return {y0 * std::cos(pitch) + z * std::sin(pitch), -y0 * std::sin(pitch) + z * std::cos(pitch)};
|
||||
}
|
||||
|
||||
// bp_plane_y, Horns_motion_4x4_ground.py: the ground plane in radar axes.
|
||||
inline auto ground_y(double z, double height, double pitch) -> double {
|
||||
if (std::abs(std::cos(pitch)) < 1e-6) { throw std::runtime_error("Ground BP is undefined at a look angle of 90 degrees"); }
|
||||
return (-height + z * std::sin(pitch)) / std::cos(pitch);
|
||||
}
|
||||
|
||||
// equalize_pairs_by_depth, Multi_depthnorm_clean.ipynb, cell 1.
|
||||
// All time axes start at zero; nearest median filtering uses the upper median
|
||||
// for even windows, as scipy.ndimage.median_filter does.
|
||||
template <class Real>
|
||||
void equalize_pairs_by_depth(const std::vector<std::span<std::complex<Real>>>& pairs, double dz, double z_max) {
|
||||
if (pairs.size() < 2U) { return; }
|
||||
std::size_t length = pairs.front().size();
|
||||
for (const auto pair : pairs) { length = std::min(length, pair.size()); }
|
||||
const double step = std::max(1.0, std::nearbyint(0.10 / dz));
|
||||
if (step > length) { return; }
|
||||
const auto d = static_cast<std::size_t>(step);
|
||||
const auto n = static_cast<std::size_t>(std::max(3.0, std::nearbyint(1.5 / (dz * d))));
|
||||
const auto nc = static_cast<std::size_t>(std::min(static_cast<double>(length), std::ceil(z_max / dz))) / d;
|
||||
if (nc < 2U) { return; }
|
||||
std::vector<std::vector<double>> levels(pairs.size(), std::vector<double>(nc));
|
||||
std::vector<double> common(nc, 0.0), envelope(nc), window(n);
|
||||
for (std::size_t a = 0; a < pairs.size(); ++a) {
|
||||
for (std::size_t i = 0; i < nc; ++i) {
|
||||
double sum = 0.0;
|
||||
for (std::size_t k = 0; k < d; ++k) { sum += std::abs(std::complex<double>(pairs[a][i * d + k])); }
|
||||
envelope[i] = sum / static_cast<double>(d);
|
||||
}
|
||||
for (std::size_t i = 0; i < nc; ++i) {
|
||||
for (std::size_t k = 0; k < n; ++k) {
|
||||
const auto index = std::clamp<std::ptrdiff_t>(static_cast<std::ptrdiff_t>(i) +
|
||||
static_cast<std::ptrdiff_t>(k) - static_cast<std::ptrdiff_t>(n / 2), 0, nc - 1);
|
||||
window[k] = envelope[index];
|
||||
}
|
||||
std::nth_element(window.begin(), window.begin() + n / 2, window.end());
|
||||
levels[a][i] = std::log(std::max(window[n / 2], 1e-30));
|
||||
common[i] += levels[a][i] / static_cast<double>(pairs.size());
|
||||
}
|
||||
}
|
||||
for (std::size_t a = 0; a < pairs.size(); ++a) {
|
||||
for (std::size_t i = 0; i < nc; ++i) { levels[a][i] = std::exp(common[i] - levels[a][i]); }
|
||||
for (std::size_t i = 0; i < nc * d; ++i) {
|
||||
const double pos = (static_cast<double>(i) - (static_cast<double>(d) - 1.0) / 2.0) / d;
|
||||
const auto j = static_cast<std::size_t>(std::clamp(std::floor(pos), 0.0, static_cast<double>(nc - 2)));
|
||||
const double w = std::clamp(pos - j, 0.0, 1.0);
|
||||
pairs[a][i] *= static_cast<Real>(levels[a][j] * (1.0 - w) + levels[a][j + 1] * w);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Scalar median normalization follows the depth equalizer in both references.
|
||||
template <class Real>
|
||||
void normalize_pair(std::span<std::complex<Real>> samples, double dz, double min_depth, double max_depth) {
|
||||
std::vector<double> magnitudes;
|
||||
for (std::size_t i = 0; i < samples.size(); ++i) {
|
||||
if (i * dz >= min_depth && i * dz <= max_depth) { magnitudes.push_back(std::abs(std::complex<double>(samples[i]))); }
|
||||
}
|
||||
if (magnitudes.empty()) {
|
||||
for (const auto sample : samples) { magnitudes.push_back(std::abs(std::complex<double>(sample))); }
|
||||
}
|
||||
if (magnitudes.empty()) { return; }
|
||||
double scale = median(std::move(magnitudes));
|
||||
if (!std::isfinite(scale) || scale <= 1e-15) { scale = 1.0; }
|
||||
for (auto& sample : samples) { sample /= static_cast<Real>(scale + 1e-15); }
|
||||
}
|
||||
|
||||
} // namespace radar::processing::bp
|
||||
@@ -93,42 +93,17 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
return std::abs(sample);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto selected_trace_samples(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> std::span<const ipc::Complex32> {
|
||||
if (live_config.bscan_channel == "s11") {
|
||||
return trace.s11;
|
||||
}
|
||||
return trace.s21;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto fallback_profile(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
std::span<const ipc::Complex32> selected_samples
|
||||
) -> BScanProfile {
|
||||
const std::size_t point_count = std::min(trace.frequency_hz.size(), selected_samples.size());
|
||||
BScanProfile fallback{};
|
||||
fallback.depth_m.reserve(point_count);
|
||||
fallback.response.reserve(point_count);
|
||||
|
||||
const float denominator = point_count > 1U ? static_cast<float>(point_count - 1U) : 1.0F;
|
||||
for (std::size_t index = 0U; index < point_count; ++index) {
|
||||
const auto& sample = selected_samples[index];
|
||||
fallback.depth_m.push_back(static_cast<float>(static_cast<float>(index) / denominator));
|
||||
fallback.response.push_back(std::sqrt(sample.re * sample.re + sample.im * sample.im));
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
|
||||
// A range profile needs at least two in-band points on an increasing frequency grid.
|
||||
// Otherwise the profile is empty and the display leaves the column out: drawing |S21|
|
||||
// against a made-up depth axis would look like a valid B-scan.
|
||||
[[nodiscard]] auto compute_bscan_profile(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> BScanProfile {
|
||||
const auto selected_samples = selected_trace_samples(trace, live_config);
|
||||
const auto selected_samples = std::span<const ipc::Complex32>(trace.s21);
|
||||
const std::size_t point_count = std::min(trace.frequency_hz.size(), selected_samples.size());
|
||||
if (point_count < 2U) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
const double configured_start_hz = static_cast<double>(live_config.bscan_start_freq_mhz) * 1'000'000.0;
|
||||
@@ -152,35 +127,35 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
}
|
||||
|
||||
if (filtered_freq_hz.size() < 2U) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
const std::size_t filtered_count = filtered_freq_hz.size();
|
||||
const double df = (filtered_freq_hz.back() - filtered_freq_hz.front()) / static_cast<double>(filtered_count - 1U);
|
||||
if (df <= 0.0) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(filtered_freq_hz.front() / df));
|
||||
if (start_bin < 0) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
const auto start_index = static_cast<std::size_t>(start_bin);
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() / 2U)) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() - filtered_count + 1U)) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
const std::size_t min_fft_len = 2U * (start_index + filtered_count - 1U);
|
||||
const std::size_t fft_len = next_power_of_two(min_fft_len);
|
||||
if (fft_len < min_fft_len || (fft_len & (fft_len - 1U)) != 0U) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
if (start_index > fft_len || filtered_count > (fft_len - start_index)) {
|
||||
return fallback_profile(trace, selected_samples);
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<std::complex<double>> spectrum(fft_len, std::complex<double>(0.0, 0.0));
|
||||
@@ -237,8 +212,6 @@ auto BScanProcessor::process_collection(
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = collection.collection_id;
|
||||
results.monotonic_ns = collection.monotonic_ns;
|
||||
results.blocks.reserve(collection.traces.size());
|
||||
|
||||
for (const auto& trace : collection.traces) {
|
||||
|
||||
+80
-398
@@ -1,5 +1,4 @@
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kSpeedOfLightMetersPerSec = 299'792'458.0;
|
||||
constexpr double kXMarginM = 2.0;
|
||||
constexpr double kGridZMinM = 0.1;
|
||||
constexpr std::size_t kGridWidth = 300U;
|
||||
@@ -7,13 +6,8 @@ constexpr std::size_t kGridHeight = 300U;
|
||||
constexpr std::size_t kAscanOversample = 8U;
|
||||
|
||||
constexpr double kRangeWeightMax = 5.0;
|
||||
constexpr double kAngleWeightMax = 2.0;
|
||||
constexpr double kTotalWeightMax = 8.0;
|
||||
constexpr double kCompensationReferenceDepthM = 5.0;
|
||||
|
||||
constexpr bool kPairNormalize = true;
|
||||
constexpr double kPairNormPercentile = 50.0;
|
||||
constexpr double kPairNormEps = 1e-15;
|
||||
|
||||
constexpr double kSmoothSigma = 1.5;
|
||||
// Gaussian-kernel half-width in sigmas, matching scipy.ndimage.gaussian_filter's
|
||||
@@ -51,19 +45,12 @@ constexpr double kScoreCfEps = 1e-12;
|
||||
|
||||
using PairKey = std::uint64_t;
|
||||
|
||||
// Frequency-domain speed correction mode (Horns_motion_3libre.py MOTION_CONFIG):
|
||||
// Frequency-domain speed correction mode:
|
||||
// IntMinus — full intra-frequency correction to the interleaved-sweep centre.
|
||||
// IntFocus — remove the constant + linear-in-frequency part of phi(f), leaving
|
||||
// the focusing residual without a net Z shift.
|
||||
enum class MotionMode { IntMinus, IntFocus };
|
||||
|
||||
[[nodiscard]] auto parse_motion_mode(const std::string& value) -> MotionMode {
|
||||
return value == "int_focus" ? MotionMode::IntFocus : MotionMode::IntMinus;
|
||||
}
|
||||
|
||||
struct GeometrySelection {
|
||||
// Per-local-index Tx/Rx antenna coordinates in metres. y_/z_ default to 0
|
||||
// for legacy configs so the imaging plane coincides with the antennas.
|
||||
// Per-local-index Tx/Rx antenna coordinates in metres.
|
||||
std::vector<std::uint32_t> input_positions{};
|
||||
std::vector<std::uint32_t> output_positions{};
|
||||
std::vector<double> x_tx{};
|
||||
@@ -76,11 +63,6 @@ struct GeometrySelection {
|
||||
std::unordered_map<std::uint32_t, std::uint32_t> output_local_by_pos{};
|
||||
};
|
||||
|
||||
struct BackgroundAccumulator {
|
||||
std::vector<std::complex<double>> sum{};
|
||||
std::size_t count = 0U;
|
||||
};
|
||||
|
||||
struct SelectedTrace {
|
||||
ipc::ComboKey combo{};
|
||||
std::uint32_t tx_local_index = 0U;
|
||||
@@ -268,20 +250,6 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
return axis;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto clamp_index(std::ptrdiff_t value, std::size_t limit) -> std::size_t {
|
||||
if (limit == 0U) {
|
||||
return 0U;
|
||||
}
|
||||
if (value < 0) {
|
||||
return 0U;
|
||||
}
|
||||
const auto max_index = static_cast<std::ptrdiff_t>(limit - 1U);
|
||||
if (value > max_index) {
|
||||
return limit - 1U;
|
||||
}
|
||||
return static_cast<std::size_t>(value);
|
||||
}
|
||||
|
||||
// scipy 'reflect' boundary (half-sample symmetric): the signal is mirrored about
|
||||
// the outer edge of the first/last sample, so index -1 maps to 0, -2 to 1, n to
|
||||
// n-1, and so on. Matches scipy.ndimage.gaussian_filter's default mode.
|
||||
@@ -314,7 +282,7 @@ void normalize_in_place(std::vector<double>& values) {
|
||||
return;
|
||||
}
|
||||
for (auto& value : values) {
|
||||
value /= maximum;
|
||||
value /= maximum + 1e-12;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -545,67 +513,10 @@ void validate_collection_trace_order(
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_background_mean(
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const GeometrySelection& selection,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> std::unordered_map<PairKey, std::vector<std::complex<double>>> {
|
||||
std::unordered_map<PairKey, std::vector<std::complex<double>>> result{};
|
||||
if (!live_config.gpr_background_subtract_enabled || live_config.gpr_background_mean_count == 0U) {
|
||||
return result;
|
||||
}
|
||||
|
||||
const std::size_t mean_count = static_cast<std::size_t>(live_config.gpr_background_mean_count);
|
||||
const std::size_t start_index =
|
||||
previous_collections.size() > mean_count ? previous_collections.size() - mean_count : 0U;
|
||||
|
||||
std::unordered_map<PairKey, BackgroundAccumulator> accumulators{};
|
||||
for (std::size_t collection_index = start_index; collection_index < previous_collections.size(); ++collection_index) {
|
||||
const auto& collection = previous_collections[collection_index];
|
||||
for (const auto& trace : collection.traces) {
|
||||
const auto output_it = selection.output_local_by_pos.find(trace.combo.output_pos);
|
||||
const auto input_it = selection.input_local_by_pos.find(trace.combo.input_pos);
|
||||
if (output_it == selection.output_local_by_pos.end() || input_it == selection.input_local_by_pos.end()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto key = make_pair_key(output_it->second, input_it->second);
|
||||
auto& accumulator = accumulators[key];
|
||||
if (accumulator.sum.empty()) {
|
||||
accumulator.sum.assign(trace.s21.size(), std::complex<double>(0.0, 0.0));
|
||||
}
|
||||
if (accumulator.sum.size() != trace.s21.size()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) {
|
||||
const auto& sample = trace.s21[sample_index];
|
||||
accumulator.sum[sample_index] += std::complex<double>(sample.re, sample.im);
|
||||
}
|
||||
accumulator.count += 1U;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto& [key, accumulator] : accumulators) {
|
||||
if (accumulator.count == 0U) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto& mean_trace = result[key];
|
||||
mean_trace = std::move(accumulator.sum);
|
||||
const double inverse_count = 1.0 / static_cast<double>(accumulator.count);
|
||||
for (auto& sample : mean_trace) {
|
||||
sample *= inverse_count;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto collect_selected_traces(
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
const GeometrySelection& selection,
|
||||
const std::unordered_map<PairKey, std::vector<std::complex<double>>>& background_mean
|
||||
const bp::Background& background_mean
|
||||
) -> std::vector<SelectedTrace> {
|
||||
std::vector<SelectedTrace> traces{};
|
||||
traces.reserve(collection.traces.size());
|
||||
@@ -637,15 +548,7 @@ void validate_collection_trace_order(
|
||||
throw std::runtime_error("GPR requires unique selected combos; duplicate combo: " + combo_to_string(trace.combo));
|
||||
}
|
||||
|
||||
const auto background_it = background_mean.find(key);
|
||||
selected.s21.reserve(trace.s21.size());
|
||||
for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) {
|
||||
std::complex<double> sample(trace.s21[sample_index].re, trace.s21[sample_index].im);
|
||||
if (background_it != background_mean.end() && background_it->second.size() == trace.s21.size()) {
|
||||
sample -= background_it->second[sample_index];
|
||||
}
|
||||
selected.s21.push_back(sample);
|
||||
}
|
||||
selected.s21 = bp::subtract_background(trace, background_mean);
|
||||
|
||||
traces.push_back(std::move(selected));
|
||||
}
|
||||
@@ -653,114 +556,6 @@ void validate_collection_trace_order(
|
||||
return traces;
|
||||
}
|
||||
|
||||
// Frequency-domain speed correction applied to S21 *before* the IFFT, ported
|
||||
// from Horns_motion_3libre.py (apply_intra_sweep_phase_correction +
|
||||
// compute_frequency_sample_times). The measurement is interleaved Tx-by-frequency
|
||||
// — Tx0(f0), Tx1(f0), Tx0(f1), ... — so each Tx occupies one interleave slot and
|
||||
// its frequency k is sampled at global step (slot_count*k + slot). Each in-band
|
||||
// frequency's phase is rotated back to the centre of the (full) interleaved sweep.
|
||||
//
|
||||
// Unlike the offline Python script, every physical input is taken from the live
|
||||
// system: `speed_mps` is the socket-fed velocity, `look_angle_deg`/`direction_sign`
|
||||
// are config, and `tx_sweep_time_s` is derived per-collection from the acquisition
|
||||
// timestamps (capture_span / Tx count). A zero speed or non-positive sweep time
|
||||
// makes this a no-op.
|
||||
void apply_intra_sweep_motion_correction(
|
||||
SelectedTrace& trace,
|
||||
std::size_t slot,
|
||||
std::size_t slot_count,
|
||||
double start_hz,
|
||||
double stop_hz,
|
||||
double velocity_mps,
|
||||
double speed_mps,
|
||||
double look_angle_deg,
|
||||
double direction_sign,
|
||||
double tx_sweep_time_s,
|
||||
MotionMode motion_mode
|
||||
) {
|
||||
const std::size_t n_full = trace.frequency_hz.size();
|
||||
if (n_full < 2U || trace.s21.size() != n_full || slot_count == 0U
|
||||
|| !(velocity_mps > 0.0) || !(tx_sweep_time_s > 0.0)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double low_hz = std::min(start_hz, stop_hz);
|
||||
const double high_hz = std::max(start_hz, stop_hz);
|
||||
|
||||
// Absolute sample time of full-sweep frequency index k for this Tx slot.
|
||||
const double dt_base = tx_sweep_time_s / static_cast<double>(n_full - 1U);
|
||||
const auto sample_time_s = [&](std::size_t index) {
|
||||
return ((static_cast<double>(slot_count) * static_cast<double>(index)) + static_cast<double>(slot)) * dt_base;
|
||||
};
|
||||
// t_center is the midpoint of the *full* interleaved sweep for this slot,
|
||||
// i.e. between the first and last full-array frequencies (not the cut band).
|
||||
const double t_center_s = 0.5 * (sample_time_s(0U) + sample_time_s(n_full - 1U));
|
||||
|
||||
const double theta_rad = (look_angle_deg * kPi) / 180.0;
|
||||
const double motion_factor = direction_sign * speed_mps * std::cos(theta_rad);
|
||||
|
||||
std::vector<std::size_t> band_indices{};
|
||||
std::vector<double> band_frequency_hz{};
|
||||
std::vector<double> phi{};
|
||||
band_indices.reserve(n_full);
|
||||
band_frequency_hz.reserve(n_full);
|
||||
phi.reserve(n_full);
|
||||
|
||||
bool any_nonzero = false;
|
||||
for (std::size_t index = 0U; index < n_full; ++index) {
|
||||
const double frequency = trace.frequency_hz[index];
|
||||
if (frequency < low_hz || frequency > high_hz) {
|
||||
continue;
|
||||
}
|
||||
const double dt_intra_s = sample_time_s(index) - t_center_s;
|
||||
const double delta_range_m = motion_factor * dt_intra_s;
|
||||
const double delta_path_m = 2.0 * delta_range_m;
|
||||
const double dtau_s = delta_path_m / velocity_mps;
|
||||
if (dtau_s != 0.0) {
|
||||
any_nonzero = true;
|
||||
}
|
||||
band_indices.push_back(index);
|
||||
band_frequency_hz.push_back(frequency);
|
||||
phi.push_back(2.0 * kPi * frequency * dtau_s);
|
||||
}
|
||||
|
||||
if (!any_nonzero || band_indices.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// int_focus removes the constant + linear-in-frequency component of phi(f).
|
||||
// With f_rel = f - mean(f) the design columns [1, f_rel] are orthogonal, so
|
||||
// the least-squares fit is intercept = mean(phi), slope = <f_rel, phi>/<f_rel, f_rel>.
|
||||
if (motion_mode == MotionMode::IntFocus) {
|
||||
const auto count = static_cast<double>(band_frequency_hz.size());
|
||||
double mean_frequency = 0.0;
|
||||
double mean_phi = 0.0;
|
||||
for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
|
||||
mean_frequency += band_frequency_hz[i];
|
||||
mean_phi += phi[i];
|
||||
}
|
||||
mean_frequency /= count;
|
||||
mean_phi /= count;
|
||||
|
||||
double cross = 0.0;
|
||||
double f_rel_sq = 0.0;
|
||||
for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
|
||||
const double f_rel = band_frequency_hz[i] - mean_frequency;
|
||||
cross += f_rel * phi[i];
|
||||
f_rel_sq += f_rel * f_rel;
|
||||
}
|
||||
const double slope = (f_rel_sq > 0.0) ? (cross / f_rel_sq) : 0.0;
|
||||
for (std::size_t i = 0U; i < phi.size(); ++i) {
|
||||
const double f_rel = band_frequency_hz[i] - mean_frequency;
|
||||
phi[i] -= mean_phi + (slope * f_rel);
|
||||
}
|
||||
}
|
||||
|
||||
for (std::size_t i = 0U; i < band_indices.size(); ++i) {
|
||||
trace.s21[band_indices[i]] *= std::polar(1.0, -phi[i]);
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto compute_ascan(
|
||||
const SelectedTrace& trace,
|
||||
double start_hz,
|
||||
@@ -806,7 +601,7 @@ void apply_intra_sweep_motion_correction(
|
||||
return result;
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(frequency_hz.front() / df_hz));
|
||||
const auto start_bin = static_cast<std::int64_t>(std::nearbyint(frequency_hz.front() / df_hz));
|
||||
if (start_bin < 0) {
|
||||
return result;
|
||||
}
|
||||
@@ -844,50 +639,6 @@ void apply_intra_sweep_motion_correction(
|
||||
return result;
|
||||
}
|
||||
|
||||
void normalize_pair_ascans(
|
||||
std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
|
||||
double velocity_mps,
|
||||
double min_depth_m,
|
||||
double max_depth_m
|
||||
) {
|
||||
if (!kPairNormalize) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (auto& item : ascans_by_pair) {
|
||||
auto& ascan = item.second;
|
||||
if (ascan.samples.empty() || ascan.time_s.size() != ascan.samples.size()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
std::vector<double> amplitudes{};
|
||||
amplitudes.reserve(ascan.samples.size());
|
||||
for (std::size_t index = 0U; index < ascan.samples.size(); ++index) {
|
||||
const double depth_m = 0.5 * velocity_mps * ascan.time_s[index];
|
||||
if (depth_m < min_depth_m || depth_m > max_depth_m) {
|
||||
continue;
|
||||
}
|
||||
amplitudes.push_back(std::abs(ascan.samples[index]));
|
||||
}
|
||||
if (amplitudes.empty()) {
|
||||
amplitudes.reserve(ascan.samples.size());
|
||||
for (const auto& sample : ascan.samples) {
|
||||
amplitudes.push_back(std::abs(sample));
|
||||
}
|
||||
}
|
||||
|
||||
double scale = percentile_copy(std::move(amplitudes), kPairNormPercentile);
|
||||
if (!std::isfinite(scale) || !(scale > kPairNormEps)) {
|
||||
scale = 1.0;
|
||||
}
|
||||
|
||||
const double denominator = scale + kPairNormEps;
|
||||
for (auto& sample : ascan.samples) {
|
||||
sample /= denominator;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto distance_3d(double dx, double dy, double dz) -> double {
|
||||
return std::sqrt((dx * dx) + (dy * dy) + (dz * dz));
|
||||
}
|
||||
@@ -896,7 +647,7 @@ void normalize_pair_ascans(
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
double imaging_plane_y_m
|
||||
double ground_depth_m, double pitch_rad
|
||||
) -> GridDefinition {
|
||||
GridDefinition grid{};
|
||||
if (selection.x_tx.empty() || selection.x_rx.empty() || !(max_depth_m > min_z_m)) {
|
||||
@@ -924,14 +675,14 @@ void normalize_pair_ascans(
|
||||
for (std::size_t tx_index = 0U; tx_index < selection.x_tx.size(); ++tx_index) {
|
||||
grid.tx_distance_grids[tx_index][cell_index] = distance_3d(
|
||||
x_value - selection.x_tx[tx_index],
|
||||
imaging_plane_y_m - selection.y_tx[tx_index],
|
||||
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_tx[tx_index],
|
||||
z_value - selection.z_tx[tx_index]
|
||||
);
|
||||
}
|
||||
for (std::size_t rx_index = 0U; rx_index < selection.x_rx.size(); ++rx_index) {
|
||||
grid.rx_distance_grids[rx_index][cell_index] = distance_3d(
|
||||
x_value - selection.x_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_rx[rx_index],
|
||||
z_value - selection.z_rx[rx_index]
|
||||
);
|
||||
}
|
||||
@@ -948,10 +699,10 @@ void normalize_pair_ascans(
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
double imaging_plane_y_m
|
||||
double ground_depth_m, double pitch_rad
|
||||
) -> std::vector<double> {
|
||||
std::vector<double> signature;
|
||||
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 3U);
|
||||
std::vector<double> signature{static_cast<double>(selection.x_tx.size()), static_cast<double>(selection.x_rx.size())};
|
||||
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 6U);
|
||||
const auto append = [&](const std::vector<double>& axis) {
|
||||
signature.insert(signature.end(), axis.begin(), axis.end());
|
||||
};
|
||||
@@ -963,7 +714,8 @@ void normalize_pair_ascans(
|
||||
append(selection.z_rx);
|
||||
signature.push_back(max_depth_m);
|
||||
signature.push_back(min_z_m);
|
||||
signature.push_back(imaging_plane_y_m);
|
||||
signature.push_back(ground_depth_m);
|
||||
signature.push_back(pitch_rad);
|
||||
return signature;
|
||||
}
|
||||
|
||||
@@ -985,67 +737,18 @@ void normalize_pair_ascans(
|
||||
return (ascan.samples[index] * (1.0 - frac)) + (ascan.samples[index + 1U] * frac);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto attenuation_components(
|
||||
double r_tx,
|
||||
double r_rx,
|
||||
double dz_tx,
|
||||
double dz_rx
|
||||
) -> std::pair<double, double> {
|
||||
// Antennas boresight along +Z, so cos(theta) = (z_pixel - z_antenna) / R.
|
||||
const double geo = 1.0 / ((r_tx * r_rx) + 1e-12);
|
||||
const double angle =
|
||||
std::pow(dz_tx / (r_tx + 1e-12), 2.0) *
|
||||
std::pow(dz_rx / (r_rx + 1e-12), 2.0);
|
||||
return {geo + 1e-30, angle + 1e-30};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto attenuation_components_at_ref_depth(
|
||||
std::uint32_t tx_index,
|
||||
std::uint32_t rx_index,
|
||||
const GeometrySelection& selection,
|
||||
double imaging_plane_y_m
|
||||
) -> std::pair<double, double> {
|
||||
const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
|
||||
const double dz_tx = kCompensationReferenceDepthM - selection.z_tx[tx_index];
|
||||
const double dz_rx = kCompensationReferenceDepthM - selection.z_rx[rx_index];
|
||||
const double r_tx = distance_3d(
|
||||
x_center - selection.x_tx[tx_index],
|
||||
imaging_plane_y_m - selection.y_tx[tx_index],
|
||||
dz_tx
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
x_center - selection.x_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
dz_rx
|
||||
);
|
||||
return attenuation_components(r_tx, r_rx, dz_tx, dz_rx);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto compensation_weight(
|
||||
double r_tx,
|
||||
double r_rx,
|
||||
double dz_tx,
|
||||
double dz_rx,
|
||||
double geo_ref,
|
||||
double angle_ref,
|
||||
double range_power,
|
||||
double angle_power
|
||||
// attenuation_geo_at_ref_depth, Horns_motion_4x4_ground.py (no angle compensation).
|
||||
[[nodiscard]] auto attenuation_geo_at_ref_depth(
|
||||
std::uint32_t tx, std::uint32_t rx, const GeometrySelection& selection,
|
||||
double ground_depth_m, double pitch_rad
|
||||
) -> double {
|
||||
const auto [geo, angle] = attenuation_components(r_tx, r_rx, dz_tx, dz_rx);
|
||||
const double geo_norm = geo / geo_ref;
|
||||
const double angle_norm = angle / angle_ref;
|
||||
|
||||
const double range_weight = std::clamp(
|
||||
1.0 / (std::pow(geo_norm, range_power) + 1e-12),
|
||||
0.0,
|
||||
kRangeWeightMax
|
||||
);
|
||||
const double angle_weight = std::clamp(
|
||||
1.0 / (std::pow(angle_norm, angle_power) + 1e-12),
|
||||
0.0,
|
||||
kAngleWeightMax
|
||||
);
|
||||
return std::clamp(range_weight * angle_weight, 0.0, kTotalWeightMax);
|
||||
const double x_center = 0.5 * (selection.x_tx[tx] + selection.x_rx[rx]);
|
||||
const double y = bp::ground_y(kCompensationReferenceDepthM, ground_depth_m, pitch_rad);
|
||||
const double r_tx = distance_3d(x_center - selection.x_tx[tx], y - selection.y_tx[tx],
|
||||
kCompensationReferenceDepthM - selection.z_tx[tx]);
|
||||
const double r_rx = distance_3d(x_center - selection.x_rx[rx], y - selection.y_rx[rx],
|
||||
kCompensationReferenceDepthM - selection.z_rx[rx]);
|
||||
return 1.0 / (r_tx * r_rx + 1e-12) + 1e-30;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto backproject_coherent(
|
||||
@@ -1053,12 +756,11 @@ void normalize_pair_ascans(
|
||||
const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
|
||||
const GridDefinition& grid,
|
||||
const GeometrySelection& selection,
|
||||
double imaging_plane_y_m,
|
||||
double ground_depth_m, double pitch_rad,
|
||||
double velocity_mps,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
double range_power,
|
||||
double angle_power
|
||||
double range_power
|
||||
) -> BpMap {
|
||||
const std::size_t width = grid.x_grid.size();
|
||||
const std::size_t height = grid.z_grid.size();
|
||||
@@ -1078,9 +780,6 @@ void normalize_pair_ascans(
|
||||
const std::vector<double>* rx_distances;
|
||||
const AscanResult* ascan;
|
||||
double geo_ref;
|
||||
double angle_ref;
|
||||
double z_tx_ant;
|
||||
double z_rx_ant;
|
||||
};
|
||||
|
||||
std::vector<PairContribution> contributions;
|
||||
@@ -1091,20 +790,17 @@ void normalize_pair_ascans(
|
||||
if (ascan_it == ascans_by_pair.end() || ascan_it->second.time_s.empty()) {
|
||||
continue;
|
||||
}
|
||||
const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth(
|
||||
const double geo_ref = attenuation_geo_at_ref_depth(
|
||||
trace.tx_local_index,
|
||||
trace.rx_local_index,
|
||||
selection,
|
||||
imaging_plane_y_m
|
||||
ground_depth_m, pitch_rad
|
||||
);
|
||||
contributions.push_back(PairContribution{
|
||||
&grid.tx_distance_grids[trace.tx_local_index],
|
||||
&grid.rx_distance_grids[trace.rx_local_index],
|
||||
&ascan_it->second,
|
||||
geo_ref,
|
||||
angle_ref,
|
||||
selection.z_tx[trace.tx_local_index],
|
||||
selection.z_rx[trace.rx_local_index],
|
||||
});
|
||||
}
|
||||
|
||||
@@ -1135,16 +831,9 @@ void normalize_pair_ascans(
|
||||
}
|
||||
|
||||
const auto sample = interpolate_complex(ascan, tau_s);
|
||||
const double weight = compensation_weight(
|
||||
r_tx,
|
||||
r_rx,
|
||||
z_m - contribution.z_tx_ant,
|
||||
z_m - contribution.z_rx_ant,
|
||||
contribution.geo_ref,
|
||||
contribution.angle_ref,
|
||||
range_power,
|
||||
angle_power
|
||||
);
|
||||
const double geo = 1.0 / (r_tx * r_rx + 1e-12) + 1e-30;
|
||||
const double weight = std::clamp(1.0 / (std::pow(geo / contribution.geo_ref, range_power) + 1e-12),
|
||||
0.0, kRangeWeightMax);
|
||||
coherent_sum += sample * weight;
|
||||
incoherent_sum += std::abs(sample) * weight;
|
||||
contribution_count += 1.0;
|
||||
@@ -1153,8 +842,8 @@ void normalize_pair_ascans(
|
||||
if (!(contribution_count > 0.0)) {
|
||||
continue;
|
||||
}
|
||||
coherent_sum /= contribution_count;
|
||||
incoherent_sum /= contribution_count;
|
||||
coherent_sum /= contribution_count + 1e-12;
|
||||
incoherent_sum /= contribution_count + 1e-12;
|
||||
result.coherent[cell_index] = coherent_sum;
|
||||
result.incoherent[cell_index] = incoherent_sum;
|
||||
result.image[cell_index] = std::abs(coherent_sum);
|
||||
@@ -1505,7 +1194,7 @@ void apply_depth_gate(
|
||||
const ObjectRecord& object,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
double imaging_plane_y_m
|
||||
double ground_depth_m, double pitch_rad
|
||||
) -> std::vector<double> {
|
||||
std::vector<double> signature{};
|
||||
signature.reserve(selected_traces.size());
|
||||
@@ -1514,12 +1203,12 @@ void apply_depth_gate(
|
||||
const auto rx = trace.rx_local_index;
|
||||
const double r_tx = distance_3d(
|
||||
object.x_m - selection.x_tx[tx],
|
||||
imaging_plane_y_m - selection.y_tx[tx],
|
||||
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_tx[tx],
|
||||
object.z_m - selection.z_tx[tx]
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
object.x_m - selection.x_rx[rx],
|
||||
imaging_plane_y_m - selection.y_rx[rx],
|
||||
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_rx[rx],
|
||||
object.z_m - selection.z_rx[rx]
|
||||
);
|
||||
signature.push_back(0.5 * (r_tx + r_rx));
|
||||
@@ -1531,12 +1220,12 @@ void mark_sidelobe_candidates(
|
||||
std::vector<ObjectRecord>& objects,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
double imaging_plane_y_m
|
||||
double ground_depth_m, double pitch_rad
|
||||
) {
|
||||
std::vector<std::vector<double>> signatures{};
|
||||
signatures.reserve(objects.size());
|
||||
for (const auto& object : objects) {
|
||||
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, imaging_plane_y_m));
|
||||
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, ground_depth_m, pitch_rad));
|
||||
}
|
||||
|
||||
for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
|
||||
@@ -1740,7 +1429,7 @@ void add_bp_score_metrics(
|
||||
}
|
||||
}
|
||||
|
||||
// Select the FINAL visible objects exactly as Horns_motion_3libre.py does, so the
|
||||
// Select the FINAL visible objects here, so the
|
||||
// processor is the single source of truth: the GUI plot and the locator socket both
|
||||
// consume this set verbatim (no second, duplicated filter). Steps, in order:
|
||||
// 1. drop sidelobe candidates (when enabled), then sort by score (peak tie-break);
|
||||
@@ -1844,8 +1533,6 @@ void add_bp_score_metrics(
|
||||
ObjectApproachFilter& approach_filter
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = collection.collection_id;
|
||||
results.monotonic_ns = collection.monotonic_ns;
|
||||
|
||||
const auto selection = build_geometry_selection(run_config, live_config);
|
||||
if (selection.input_positions.empty() || selection.output_positions.empty()) {
|
||||
@@ -1853,16 +1540,15 @@ void add_bp_score_metrics(
|
||||
}
|
||||
|
||||
validate_collection_trace_order(run_config, collection);
|
||||
const auto background_mean = build_background_mean(previous_collections, selection, live_config);
|
||||
const auto background_mean = bp::background_mean(previous_collections, live_config.gpr_background_subtract_enabled,
|
||||
live_config.gpr_background_mean_count);
|
||||
auto selected_traces = collect_selected_traces(collection, selection, background_mean);
|
||||
if (selected_traces.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
// Coherent BP fixes the medium to vacuum/air (eps_r = 1), matching the Python
|
||||
// reference Horns_motion_3libre.py (its 0.3 block hardcodes eps_r = 1.0). The
|
||||
// configurable relative_permittivity stays a legacy-GPR-only knob.
|
||||
const double velocity_mps = kSpeedOfLightMetersPerSec;
|
||||
// Coherent BP propagates in air, as in Horns_motion_4x4_ground.py.
|
||||
const double velocity_mps = bp::kWaveSpeed;
|
||||
const double start_hz = static_cast<double>(live_config.gpr_start_freq_mhz) * 1'000'000.0;
|
||||
const double stop_hz = static_cast<double>(live_config.gpr_stop_freq_mhz) * 1'000'000.0;
|
||||
const double min_depth_m = static_cast<double>(live_config.gpr_min_depth_m);
|
||||
@@ -1871,36 +1557,12 @@ void add_bp_score_metrics(
|
||||
return results;
|
||||
}
|
||||
|
||||
// Intra-sweep speed correction before the IFFT. The Tx interleave slot is the
|
||||
// Tx local index (outputs are sorted), with slot_count = number of selected Tx.
|
||||
// tx_sweep_time is derived per-collection from the acquisition timestamps:
|
||||
// one Tx's interleaved sweep spans the whole frame, so capture_span / N_tx.
|
||||
// Speed comes from the socket-fed live config; look-angle/direction from config.
|
||||
const auto motion_mode = parse_motion_mode(live_config.gpr_motion_mode);
|
||||
const std::size_t motion_slot_count = selection.output_positions.size();
|
||||
const double capture_span_s =
|
||||
collection.capture_end_ns > collection.capture_start_ns
|
||||
? static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9
|
||||
: 0.0;
|
||||
const double tx_sweep_time_s =
|
||||
motion_slot_count > 0U ? capture_span_s / static_cast<double>(motion_slot_count) : 0.0;
|
||||
const double motion_speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
|
||||
const double motion_look_angle_deg = static_cast<double>(live_config.gpr_look_angle_deg);
|
||||
const double motion_direction_sign = static_cast<double>(live_config.gpr_direction_sign);
|
||||
const auto timing = bp::collection_timing(collection);
|
||||
const double motion_look_angle_deg = live_config.gpr_look_angle_deg;
|
||||
for (auto& trace : selected_traces) {
|
||||
apply_intra_sweep_motion_correction(
|
||||
trace,
|
||||
trace.tx_local_index,
|
||||
motion_slot_count,
|
||||
start_hz,
|
||||
stop_hz,
|
||||
velocity_mps,
|
||||
motion_speed_mps,
|
||||
motion_look_angle_deg,
|
||||
motion_direction_sign,
|
||||
tx_sweep_time_s,
|
||||
motion_mode
|
||||
);
|
||||
bp::apply_motion_phase(trace.s21, collection.traces[trace.run_order].frequency_hz, timing.clocks[trace.run_order],
|
||||
live_config.gpr_speed_m_s, motion_look_angle_deg * kPi / 180.0,
|
||||
live_config.gpr_direction_sign, live_config.gpr_motion_mode == "int_focus", start_hz, stop_hz);
|
||||
}
|
||||
|
||||
std::unordered_map<PairKey, AscanResult> ascans_by_pair{};
|
||||
@@ -1917,9 +1579,30 @@ void add_bp_score_metrics(
|
||||
if (ascans_by_pair.empty()) {
|
||||
return results;
|
||||
}
|
||||
normalize_pair_ascans(ascans_by_pair, velocity_mps, min_depth_m, max_depth_m);
|
||||
|
||||
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
|
||||
std::vector<std::span<std::complex<double>>> pairs;
|
||||
for (const auto& trace : selected_traces) {
|
||||
const auto found = ascans_by_pair.find(make_pair_key(trace.tx_local_index, trace.rx_local_index));
|
||||
if (found != ascans_by_pair.end()) { pairs.emplace_back(found->second.samples); }
|
||||
}
|
||||
const double dt = ascans_by_pair.begin()->second.dt_s;
|
||||
double x_min = selection.x_tx.front(), x_max = x_min, max_abs_x = 0.0;
|
||||
for (const auto* xs : {&selection.x_tx, &selection.x_rx}) {
|
||||
for (const double x : *xs) {
|
||||
x_min = std::min(x_min, x); x_max = std::max(x_max, x); max_abs_x = std::max(max_abs_x, std::abs(x));
|
||||
}
|
||||
}
|
||||
if (live_config.pair_depth_equalize) {
|
||||
for (const auto& [key, ascan] : ascans_by_pair) {
|
||||
if (ascan.dt_s != dt) { throw std::runtime_error("PAIR_DEPTH_EQUALIZE requires a common frequency grid"); }
|
||||
}
|
||||
bp::equalize_pairs_by_depth(pairs, dt * velocity_mps / 2.0,
|
||||
bp::equalize_depth_limit(max_depth_m, x_min - kXMarginM, x_max + kXMarginM, max_abs_x));
|
||||
}
|
||||
for (auto& [key, ascan] : ascans_by_pair) {
|
||||
bp::normalize_pair(std::span(ascan.samples), ascan.dt_s * velocity_mps / 2.0, min_depth_m, max_depth_m);
|
||||
}
|
||||
const double ground_depth_m = live_config.gpr_bp_ground_depth_m;
|
||||
const double pitch_rad = motion_look_angle_deg * kPi / 180.0;
|
||||
|
||||
// The imaging grid (90k cells x 6 distance fields) only depends on antenna
|
||||
// geometry and the depth window, which are constant across a run, so it is
|
||||
@@ -1928,9 +1611,9 @@ void add_bp_score_metrics(
|
||||
// backprojection workers join before it returns), so a local static is safe.
|
||||
static std::vector<double> cached_grid_signature;
|
||||
static GridDefinition cached_grid;
|
||||
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, ground_depth_m, pitch_rad);
|
||||
if (cached_grid.x_grid.empty() || signature != cached_grid_signature) {
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, ground_depth_m, pitch_rad);
|
||||
cached_grid_signature = signature;
|
||||
}
|
||||
const GridDefinition& grid = cached_grid;
|
||||
@@ -1943,12 +1626,11 @@ void add_bp_score_metrics(
|
||||
ascans_by_pair,
|
||||
grid,
|
||||
selection,
|
||||
imaging_plane_y_m,
|
||||
ground_depth_m, pitch_rad,
|
||||
velocity_mps,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
std::max(0.0, static_cast<double>(live_config.gpr_range_comp_power)),
|
||||
0.0 // angle compensation fixed off (Python Horns_motion_3libre.py COMP_ANGLE_POWER = 0.0)
|
||||
std::max(0.0, static_cast<double>(live_config.gpr_range_comp_power))
|
||||
);
|
||||
if (bp.image.empty()) {
|
||||
return results;
|
||||
@@ -1961,7 +1643,7 @@ void add_bp_score_metrics(
|
||||
auto objects = find_bp_objects(display_map, grid, static_cast<double>(live_config.gpr_object_min_frac));
|
||||
add_local_prominence_metrics(objects, display_map, grid, min_depth_m, max_depth_m);
|
||||
add_incoherent_support_metrics(objects, incoherent_display_map, bp.coherence_factor);
|
||||
mark_sidelobe_candidates(objects, selected_traces, selection, imaging_plane_y_m);
|
||||
mark_sidelobe_candidates(objects, selected_traces, selection, ground_depth_m, pitch_rad);
|
||||
add_bp_score_metrics(objects, live_config);
|
||||
|
||||
if (live_config.gpr_remove_sidelobe_objects_enabled) {
|
||||
@@ -1980,7 +1662,7 @@ void add_bp_score_metrics(
|
||||
results.collection_payloads.push_back(build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, display_map));
|
||||
|
||||
// Final visible set (window + N/M), then the cross-frame approach filter: keep only
|
||||
// objects confirmed as a >= min_frames motion-consistent track (Horns_motion notebook).
|
||||
// objects confirmed as a >= min_frames motion-consistent track.
|
||||
const auto visible = select_visible_objects(objects, live_config);
|
||||
std::vector<ObjectApproachFilter::Point> visible_points{};
|
||||
visible_points.reserve(visible.size());
|
||||
@@ -1990,7 +1672,7 @@ void add_bp_score_metrics(
|
||||
const auto confirmed = approach_filter.confirm(
|
||||
visible_points,
|
||||
collection.collection_id,
|
||||
static_cast<double>(collection.monotonic_ns) * 1e-9,
|
||||
collection.capture_start_ns,
|
||||
static_cast<double>(live_config.gpr_speed_m_s),
|
||||
static_cast<double>(live_config.gpr_look_angle_deg),
|
||||
static_cast<std::size_t>(live_config.gpr_object_approach_min_frames)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,4 +1,5 @@
|
||||
#include "gpr_processor.hpp"
|
||||
#include "backprojection.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
@@ -22,7 +23,6 @@ namespace {
|
||||
|
||||
// Algorithm bodies stay private to this translation unit; the public processor remains a small dispatcher.
|
||||
#include "gpr_backprojection_processor.ipp"
|
||||
#include "gpr_legacy_processor.ipp"
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -39,17 +39,4 @@ auto GprProcessor::process_collection(
|
||||
return process_backprojection_gpr(run_config, collection, previous_collections, live_config, approach_filter_);
|
||||
}
|
||||
|
||||
auto LegacyGprProcessor::name() const -> std::string {
|
||||
return "legacy_gpr";
|
||||
}
|
||||
|
||||
auto LegacyGprProcessor::process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
return process_legacy_gpr(run_config, collection, previous_collections, live_config);
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
|
||||
@@ -10,11 +10,9 @@ auto PassThroughProcessor::process_collection(
|
||||
const config::RunConfig& /*run_config*/,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> /*previous_collections*/,
|
||||
const ProcessingLiveConfig& live_config
|
||||
const ProcessingLiveConfig& /*live_config*/
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = collection.collection_id;
|
||||
results.monotonic_ns = collection.monotonic_ns;
|
||||
results.blocks.reserve(collection.traces.size());
|
||||
|
||||
for (const auto& trace : collection.traces) {
|
||||
@@ -22,11 +20,7 @@ auto PassThroughProcessor::process_collection(
|
||||
payload.processing_name = name();
|
||||
payload.kind = ipc::ResultKind::TraceComplex;
|
||||
payload.frequency_hz = trace.frequency_hz;
|
||||
if (live_config.pass_through_channel == "s11") {
|
||||
payload.trace = trace.s11;
|
||||
} else {
|
||||
payload.trace = trace.s21;
|
||||
}
|
||||
payload.trace = trace.s21;
|
||||
|
||||
ipc::ResultBlock block{};
|
||||
block.combo = trace.combo;
|
||||
|
||||
@@ -0,0 +1,558 @@
|
||||
#include "sar3d_clean.hpp"
|
||||
#include "backprojection.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <iterator>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
// Port of the CLEAN and frame parts of notebook cell 5. Comments name the notebook's
|
||||
// functions and constants; the constants are values the notebook checked on scenes with
|
||||
// several objects, not settings.
|
||||
namespace radar::processing::sar3d {
|
||||
namespace {
|
||||
|
||||
// CLEAN (sar_clean_components, sar_clean_frame).
|
||||
constexpr double kCleanStop = 5.5; // SAR_CLEAN_STOP_BG, backgrounds of the whole volume
|
||||
constexpr double kCleanRangeStop = 4.5; // SAR_CLEAN_STOP_RANGE_BG, backgrounds at the voxel range
|
||||
constexpr std::size_t kCleanMaxSteps = 15U; // SAR_CLEAN_MAX_STEPS
|
||||
constexpr std::array<double, 3> kCleanMerge{0.35, 0.50, 0.40}; // _SAR_CLEAN_MERGE_M (X, Y, Z)
|
||||
constexpr std::array<double, 3> kCleanBox{0.40, 0.50, 0.45}; // _SAR_CLEAN_BOX_M (X, Y, Z), half-size
|
||||
constexpr double kCleanPick = 0.05; // _SAR_CLEAN_PICK_M, pair peak window around the delay
|
||||
constexpr double kCleanSearchSigma = 1.0; // _SAR_CLEAN_SEARCH_SIGMA, voxels
|
||||
constexpr double kCleanRing = 0.25; // _SAR_CLEAN_RING_M, range ring width
|
||||
constexpr std::size_t kCleanRingSmooth = 5U; // _SAR_CLEAN_RING_SMOOTH, rings
|
||||
constexpr std::size_t kCleanRingMinVoxels = 300U; // _SAR_CLEAN_RING_MIN_VOXELS
|
||||
|
||||
// Boxes (sar_mark_groups).
|
||||
constexpr double kMarkLevel = 0.6; // _SAR_MARK_LEVEL, response height level of the peak
|
||||
constexpr double kMarkMemberRel = 0.4; // _SAR_MARK_MEMBER_REL
|
||||
constexpr double kMarkPad = 0.25; // _SAR_MARK_PAD_M, along X and Z
|
||||
constexpr double kMarkEdgeFar = 0.25; // _SAR_MARK_EDGE_FAR_M
|
||||
constexpr double kMarkEdgeNear = 0.10; // _SAR_MARK_EDGE_NEAR_M
|
||||
constexpr double kMarkEdgeX = 0.20; // _SAR_MARK_EDGE_X_M
|
||||
constexpr double kMarkEdgeTop = 0.40; // _SAR_MARK_EDGE_TOP_M
|
||||
constexpr std::array<double, 2> kMarkGroup{0.8, 0.5}; // SAR_MARK_GROUP_M (X, Z)
|
||||
constexpr double kMarkGround = 0.35; // SAR_MARK_GROUND_M
|
||||
constexpr double kMarkLow = 0.15; // SAR_MARK_LOW_M
|
||||
constexpr double kMarkLowTop = 0.45; // SAR_MARK_LOW_TOP_M
|
||||
|
||||
using Scans = std::vector<std::complex<float>>;
|
||||
|
||||
// numpy.interp: linear between the points of increasing `xp`, constant beyond them.
|
||||
[[nodiscard]] auto interp(double x, std::span<const double> xp, std::span<const double> fp) -> double {
|
||||
if (x <= xp.front()) {
|
||||
return fp.front();
|
||||
}
|
||||
if (x >= xp.back()) {
|
||||
return fp.back();
|
||||
}
|
||||
const auto j = static_cast<std::size_t>(std::ranges::upper_bound(xp, x) - xp.begin()) - 1U;
|
||||
if (xp[j] == x) {
|
||||
return fp[j];
|
||||
}
|
||||
return (fp[j + 1U] - fp[j]) / (xp[j + 1U] - xp[j]) * (x - xp[j]) + fp[j];
|
||||
}
|
||||
|
||||
// Radar axes (Y, Z) of a scene point of the frame.
|
||||
[[nodiscard]] auto radar_yz(const VolumeRequest& frame, double y, double z) -> std::array<double, 2> {
|
||||
return bp::radar_yz(y, z, frame.ground_depth_m, frame.pitch_rad);
|
||||
}
|
||||
|
||||
// Axis indices [first, first + count).
|
||||
struct Range {
|
||||
std::size_t first = 0U;
|
||||
std::size_t count = 0U;
|
||||
};
|
||||
|
||||
// The axis points within `half_width` of `center`; they are contiguous on a monotonic axis.
|
||||
[[nodiscard]] auto within(std::span<const double> axis, double center, double half_width) -> Range {
|
||||
Range range;
|
||||
for (std::size_t index = 0U; index < axis.size(); ++index) {
|
||||
if (std::abs(axis[index] - center) <= half_width) {
|
||||
if (range.count == 0U) {
|
||||
range.first = index;
|
||||
}
|
||||
range.count = index - range.first + 1U;
|
||||
}
|
||||
}
|
||||
return range;
|
||||
}
|
||||
|
||||
// scan += amplitude * numpy.roll(pulse, shift)
|
||||
void add_pulse(std::span<std::complex<float>> scan, std::span<const std::complex<float>> pulse, std::size_t shift,
|
||||
std::complex<double> amplitude) {
|
||||
const std::size_t length = scan.size();
|
||||
for (std::size_t sample = 0U; sample < length; ++sample) {
|
||||
const std::complex<double> response = pulse[(sample + length - shift) % length];
|
||||
scan[sample] = std::complex<float>(std::complex<double>(scan[sample]) + amplitude * response);
|
||||
}
|
||||
}
|
||||
|
||||
// Range rings of the search grid (sar_clean_range_rings): every voxel's range from the
|
||||
// radar, the ring centres and the working voxels (depth gate, reference planes) of each ring.
|
||||
struct Rings {
|
||||
std::vector<double> range;
|
||||
std::vector<double> centers;
|
||||
std::vector<std::vector<std::size_t>> members;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto range_rings(const VolumeRequest& frame, const Axes& grid) -> Rings {
|
||||
Rings rings;
|
||||
std::vector<std::size_t> work;
|
||||
for (std::size_t iy = 0U; iy < grid.y.size(); ++iy) {
|
||||
const bool reference = reference_plane(grid.y, frame.norm_edge_margin_m, iy);
|
||||
for (const double z : grid.z) {
|
||||
const auto [y_radar, z_radar] = radar_yz(frame, grid.y[iy], z);
|
||||
const bool working = reference && z_radar >= frame.min_depth_m && z_radar <= frame.max_depth_m;
|
||||
for (const double x : grid.x) {
|
||||
if (working) {
|
||||
work.push_back(rings.range.size());
|
||||
}
|
||||
rings.range.push_back(std::sqrt(x * x + y_radar * y_radar + z_radar * z_radar));
|
||||
}
|
||||
}
|
||||
}
|
||||
// Ring edges numpy.arange(0, R.max() + 2 ring, ring); the voxel ring is numpy.digitize - 1, clipped.
|
||||
const auto edges = static_cast<std::size_t>(
|
||||
std::ceil((*std::ranges::max_element(rings.range) + 2.0 * kCleanRing) / kCleanRing));
|
||||
rings.members.resize(edges - 1U);
|
||||
for (std::size_t ring = 0U; ring + 1U < edges; ++ring) {
|
||||
rings.centers.push_back((static_cast<double>(ring) + 0.5) * kCleanRing);
|
||||
}
|
||||
for (const auto voxel : work) {
|
||||
const auto ring = std::min(static_cast<std::size_t>(rings.range[voxel] / kCleanRing), edges - 2U);
|
||||
rings.members[ring].push_back(voxel);
|
||||
}
|
||||
return rings;
|
||||
}
|
||||
|
||||
// Background at the centre of every ring (sar_clean_range_background): the median of the
|
||||
// ring, filled in over sparse rings and averaged over neighbouring ones; empty when no ring
|
||||
// has enough voxels.
|
||||
[[nodiscard]] auto range_background(std::span<const float> volume, const Rings& rings) -> std::vector<double> {
|
||||
std::vector<double> known_rings;
|
||||
std::vector<double> known_medians;
|
||||
for (std::size_t ring = 0U; ring < rings.members.size(); ++ring) {
|
||||
const auto& members = rings.members[ring];
|
||||
if (members.size() >= kCleanRingMinVoxels) {
|
||||
std::vector<float> values;
|
||||
values.reserve(members.size());
|
||||
for (const auto voxel : members) {
|
||||
values.push_back(volume[voxel]);
|
||||
}
|
||||
known_rings.push_back(static_cast<double>(ring));
|
||||
known_medians.push_back(bp::median(std::move(values)));
|
||||
}
|
||||
}
|
||||
if (known_rings.empty()) {
|
||||
return {};
|
||||
}
|
||||
const std::size_t count = rings.members.size();
|
||||
std::vector<double> medians(count);
|
||||
for (std::size_t ring = 0U; ring < count; ++ring) {
|
||||
medians[ring] = interp(static_cast<double>(ring), known_rings, known_medians);
|
||||
}
|
||||
// Moving average over kCleanRingSmooth rings with edge padding (numpy.convolve 'valid'
|
||||
// with numpy.ones(k) / k).
|
||||
const auto half = static_cast<std::ptrdiff_t>(kCleanRingSmooth / 2U);
|
||||
const auto last = static_cast<std::ptrdiff_t>(count) - 1;
|
||||
const double weight = 1.0 / static_cast<double>(kCleanRingSmooth);
|
||||
std::vector<double> smoothed(count);
|
||||
for (std::size_t ring = 0U; ring < count; ++ring) {
|
||||
for (std::ptrdiff_t tap = -half; tap <= half; ++tap) {
|
||||
const auto index = std::clamp(static_cast<std::ptrdiff_t>(ring) + tap, std::ptrdiff_t{0}, last);
|
||||
smoothed[ring] += medians[static_cast<std::size_t>(index)] * weight;
|
||||
}
|
||||
}
|
||||
return smoothed;
|
||||
}
|
||||
|
||||
// The search voxel highest above its stop threshold, the lower of kCleanStop backgrounds
|
||||
// of the whole volume and kCleanRangeStop backgrounds at its range; its value in units of
|
||||
// that threshold. Empty voxels score 0.
|
||||
[[nodiscard]] auto strongest(std::span<const float> volume, const Rings& rings) -> std::pair<std::size_t, double> {
|
||||
std::vector<float> positive;
|
||||
std::ranges::copy_if(volume, std::back_inserter(positive), [](float value) { return value > 0.0F; });
|
||||
const double overall = positive.empty() ? 0.0 : kCleanStop * bp::median(std::move(positive));
|
||||
const auto local = range_background(volume, rings);
|
||||
std::pair<std::size_t, double> best{0U, 0.0};
|
||||
for (std::size_t voxel = 0U; voxel < volume.size(); ++voxel) {
|
||||
if (!(volume[voxel] > 0.0F)) {
|
||||
continue;
|
||||
}
|
||||
double threshold = overall;
|
||||
if (!local.empty()) {
|
||||
threshold = std::min(overall, kCleanRangeStop * interp(rings.range[voxel], rings.centers, local));
|
||||
}
|
||||
const double score = volume[voxel] / (threshold + 1e-30);
|
||||
if (score > best.second) {
|
||||
best = {voxel, score};
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
// One subtracted point response: the pair, its peak sample and the pulse amplitude there.
|
||||
struct Part {
|
||||
std::size_t pair = 0U;
|
||||
std::size_t sample = 0U;
|
||||
std::complex<double> amplitude{};
|
||||
};
|
||||
|
||||
struct Component {
|
||||
std::array<double, 3> point{}; // search voxel, scene X, Y, Z
|
||||
std::vector<Part> parts;
|
||||
};
|
||||
|
||||
// CLEAN steps on the search grid (sar_clean_components). Each step takes the search voxel
|
||||
// furthest above its stop threshold and subtracts a point response from the A-scan of every
|
||||
// pair at the pair's own peak near the voxel delay; `residual` keeps what remains.
|
||||
template <class Image>
|
||||
[[nodiscard]] auto subtract_components(const Image& image, const CleanRequest& request, Scans& residual)
|
||||
-> std::vector<Component> {
|
||||
const VolumeRequest& frame = request.frame;
|
||||
const Axes& search = request.search;
|
||||
const std::size_t length = request.pulse.size();
|
||||
const std::size_t plane = search.x.size() * search.z.size();
|
||||
const auto rings = range_rings(frame, search);
|
||||
// Half-width of the pair peak window in samples: _SAR_CLEAN_PICK_M of range, which is
|
||||
// twice that of Tx+Rx path.
|
||||
const auto pick = std::max<std::ptrdiff_t>(
|
||||
1, static_cast<std::ptrdiff_t>(std::nearbyint(2.0 * kCleanPick * frame.samples_per_metre)));
|
||||
std::vector<Component> components;
|
||||
while (true) {
|
||||
auto volume = image(residual, search, kCleanSearchSigma);
|
||||
for (std::size_t iy = 0U; iy < search.y.size(); ++iy) {
|
||||
if (!reference_plane(search.y, frame.norm_edge_margin_m, iy)) {
|
||||
std::fill_n(volume.begin() + static_cast<std::ptrdiff_t>(iy * plane), plane, 0.0F);
|
||||
}
|
||||
}
|
||||
const auto [voxel, level] = strongest(volume, rings);
|
||||
if (level < 1.0 || components.size() >= kCleanMaxSteps) {
|
||||
return components;
|
||||
}
|
||||
const std::size_t ix = voxel % search.x.size();
|
||||
const std::size_t iz = voxel / search.x.size() % search.z.size();
|
||||
const std::size_t iy = voxel / plane;
|
||||
Component component{.point = {search.x[ix], search.y[iy], search.z[iz]}, .parts = {}};
|
||||
const auto [y_radar, z_radar] = radar_yz(frame, search.y[iy], search.z[iz]);
|
||||
const std::array<double, 3> target{search.x[ix], y_radar, z_radar}; // in radar axes
|
||||
const auto range = [&target](const std::array<float, 3>& antenna) {
|
||||
const double dx = target[0] - antenna[0];
|
||||
const double dy = target[1] - antenna[1];
|
||||
const double dz = target[2] - antenna[2];
|
||||
return std::sqrt(dx * dx + dy * dy + dz * dz);
|
||||
};
|
||||
for (std::size_t pair = 0U; pair < frame.contributions.size(); ++pair) {
|
||||
const auto& term = frame.contributions[pair];
|
||||
// Sample of the voxel's Tx+Rx path (sar_clean_pair_delays).
|
||||
const auto delay = static_cast<std::ptrdiff_t>(
|
||||
std::nearbyint((range(term.tx) + range(term.rx)) * frame.samples_per_metre));
|
||||
const auto low = std::max<std::ptrdiff_t>(delay - pick, 0);
|
||||
const auto high = std::min(delay + pick + 1, static_cast<std::ptrdiff_t>(length));
|
||||
if (high <= low) {
|
||||
continue;
|
||||
}
|
||||
const auto scan = std::span(residual).subspan(pair * length, length);
|
||||
auto peak = static_cast<std::size_t>(low);
|
||||
for (auto sample = peak + 1U; sample < static_cast<std::size_t>(high); ++sample) {
|
||||
if (std::norm(std::complex<double>(scan[sample])) > std::norm(std::complex<double>(scan[peak]))) {
|
||||
peak = sample;
|
||||
}
|
||||
}
|
||||
const auto amplitude = std::complex<double>(scan[peak]) / std::complex<double>(request.pulse[0]);
|
||||
add_pulse(scan, request.pulse, peak, -amplitude);
|
||||
component.parts.push_back({.pair = pair, .sample = peak, .amplitude = amplitude});
|
||||
}
|
||||
components.push_back(std::move(component));
|
||||
}
|
||||
}
|
||||
|
||||
// Subtractions within _SAR_CLEAN_MERGE_M of an object's first subtraction belong to that
|
||||
// object (sar_clean_group).
|
||||
struct Group {
|
||||
std::array<double, 3> seed{};
|
||||
std::vector<const Component*> members;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto group(const std::vector<Component>& components) -> std::vector<Group> {
|
||||
std::vector<Group> groups;
|
||||
for (const auto& component : components) {
|
||||
const auto near_seed = [&component](const Group& candidate) {
|
||||
for (std::size_t axis = 0U; axis < 3U; ++axis) {
|
||||
if (std::abs(component.point[axis] - candidate.seed[axis]) > kCleanMerge[axis]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
};
|
||||
if (const auto found = std::ranges::find_if(groups, near_seed); found != groups.end()) {
|
||||
found->members.push_back(&component);
|
||||
} else {
|
||||
groups.push_back({.seed = component.point, .members = {&component}});
|
||||
}
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
auto frame_background(std::span<const float> values, std::span<const float> volume, const VolumeRequest& frame)
|
||||
-> FrameBackground {
|
||||
const Axes& grid = frame.grid;
|
||||
const std::size_t plane = grid.x.size() * grid.z.size();
|
||||
if (values.size() != volume.size() || volume.size() != plane * grid.y.size()) {
|
||||
throw std::runtime_error("sar3d background volumes do not match the frame grid");
|
||||
}
|
||||
std::vector<float> work;
|
||||
work.reserve(volume.size());
|
||||
FrameBackground result;
|
||||
for (std::size_t iy = 0U; iy < grid.y.size(); ++iy) {
|
||||
if (!reference_plane(grid.y, frame.norm_edge_margin_m, iy)) {
|
||||
continue;
|
||||
}
|
||||
for (std::size_t voxel = iy * plane; voxel < (iy + 1U) * plane; ++voxel) {
|
||||
if (volume[voxel] > 0.0F) {
|
||||
work.push_back(values[voxel]);
|
||||
result.peak = std::max(result.peak, static_cast<double>(values[voxel]));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!work.empty()) {
|
||||
result.median = bp::median(std::move(work));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
auto clean(Gpu& gpu, const CleanRequest& request) -> Clean {
|
||||
const VolumeRequest& frame = request.frame;
|
||||
const Axes& grid = frame.grid;
|
||||
const std::size_t pairs = frame.contributions.size();
|
||||
const std::size_t length = request.pulse.size();
|
||||
if (pairs == 0U || request.ascans.size() != pairs * length ||
|
||||
request.volume.size() != grid.x.size() * grid.y.size() * grid.z.size() || !(request.level > 0.0)) {
|
||||
throw std::runtime_error("sar3d CLEAN input does not match its frame");
|
||||
}
|
||||
// Smoothed |BP| of the frame pairs with other A-scans on another grid (sar_abs_volume).
|
||||
// The A-scans are full-length: the search grid may reach past the frame grid that the
|
||||
// frame's shortened A-scans cover.
|
||||
const auto image = [&gpu, &frame, length](const Scans& scans, const Axes& axes, double sigma) {
|
||||
VolumeRequest imaged = frame;
|
||||
imaged.grid = axes;
|
||||
imaged.ascans = scans;
|
||||
imaged.samples_per_ascan = length;
|
||||
imaged.smooth_sigma_voxels = sigma;
|
||||
gpu.image(imaged);
|
||||
return gpu.download();
|
||||
};
|
||||
|
||||
Scans residual(request.ascans.begin(), request.ascans.end());
|
||||
const auto components = subtract_components(image, request, residual);
|
||||
Clean result;
|
||||
result.steps = components.size();
|
||||
if (components.empty()) {
|
||||
result.residual_ratio = request.frame_background.ratio();
|
||||
return result;
|
||||
}
|
||||
// The residual in units of the frame maximum, hidden when it is background.
|
||||
const auto residual_image = image(residual, grid, frame.smooth_sigma_voxels);
|
||||
result.residual_ratio = frame_background(residual_image, request.volume, frame).ratio();
|
||||
result.display.assign(residual_image.size(), 0.0F);
|
||||
if (!request.bg_gate || result.residual_ratio >= kBgGateShow) {
|
||||
std::ranges::transform(residual_image, result.display.begin(),
|
||||
[&request](float value) { return static_cast<float>(value / request.level); });
|
||||
}
|
||||
|
||||
const std::size_t grid_nx = grid.x.size();
|
||||
const std::size_t grid_nz = grid.z.size();
|
||||
for (const auto& object : group(components)) {
|
||||
const auto& seed = object.seed;
|
||||
// The object apart from the others: the residual plus its own subtractions, imaged on
|
||||
// the frame grid around it only (sar_clean_subgrid).
|
||||
const Range window_x = within(grid.x, seed[0], 2.0 * kCleanBox[0]);
|
||||
const Range window_y = within(grid.y, seed[1], 2.0 * kCleanBox[1]);
|
||||
const Range window_z = within(grid.z, seed[2], 2.0 * kCleanBox[2]);
|
||||
if (window_x.count < 2U || window_y.count < 2U || window_z.count < 2U) {
|
||||
continue;
|
||||
}
|
||||
const Axes local{
|
||||
.x = grid.x.subspan(window_x.first, window_x.count),
|
||||
.y = grid.y.subspan(window_y.first, window_y.count),
|
||||
.z = grid.z.subspan(window_z.first, window_z.count),
|
||||
};
|
||||
Scans scans = residual;
|
||||
for (const Component* member : object.members) {
|
||||
for (const Part& part : member->parts) {
|
||||
add_pulse(std::span(scans).subspan(part.pair * length, length), request.pulse, part.sample,
|
||||
part.amplitude);
|
||||
}
|
||||
}
|
||||
const auto values = image(scans, local, frame.smooth_sigma_voxels);
|
||||
const std::size_t nx = local.x.size();
|
||||
const std::size_t nz = local.z.size();
|
||||
const auto at = [&values, nx, nz](std::size_t iy, std::size_t iz, std::size_t ix) {
|
||||
return values[(iy * nz + iz) * nx + ix];
|
||||
};
|
||||
|
||||
// Peak within _SAR_CLEAN_BOX_M of the first subtraction.
|
||||
const Range near_x = within(local.x, seed[0], kCleanBox[0]);
|
||||
const Range near_y = within(local.y, seed[1], kCleanBox[1]);
|
||||
const Range near_z = within(local.z, seed[2], kCleanBox[2]);
|
||||
if (near_x.count == 0U || near_y.count == 0U || near_z.count == 0U) {
|
||||
continue;
|
||||
}
|
||||
std::size_t py = near_y.first;
|
||||
std::size_t pz = near_z.first;
|
||||
std::size_t px = near_x.first;
|
||||
for (std::size_t iy = near_y.first; iy < near_y.first + near_y.count; ++iy) {
|
||||
for (std::size_t iz = near_z.first; iz < near_z.first + near_z.count; ++iz) {
|
||||
for (std::size_t ix = near_x.first; ix < near_x.first + near_x.count; ++ix) {
|
||||
if (at(iy, iz, ix) > at(py, pz, px)) {
|
||||
py = iy;
|
||||
pz = iz;
|
||||
px = ix;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
const double peak = at(py, pz, px);
|
||||
if (!(peak > 0.0)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Height of the response: where the maximum over 5 x 5 voxels in Z and X around the
|
||||
// peak stays at _SAR_MARK_LEVEL of the peak.
|
||||
const auto column = [&](std::size_t iy) {
|
||||
float maximum = 0.0F;
|
||||
for (std::size_t iz = pz < 2U ? 0U : pz - 2U; iz < std::min(pz + 3U, nz); ++iz) {
|
||||
for (std::size_t ix = px < 2U ? 0U : px - 2U; ix < std::min(px + 3U, nx); ++ix) {
|
||||
maximum = std::max(maximum, at(iy, iz, ix));
|
||||
}
|
||||
}
|
||||
return maximum / peak;
|
||||
};
|
||||
std::size_t y_low = py;
|
||||
std::size_t y_high = py;
|
||||
while (y_low > 0U && column(y_low - 1U) >= kMarkLevel) {
|
||||
--y_low;
|
||||
}
|
||||
while (y_high + 1U < local.y.size() && column(y_high + 1U) >= kMarkLevel) {
|
||||
++y_high;
|
||||
}
|
||||
|
||||
// The object in its own 0-1 scale over the box around its peak.
|
||||
const Range box_x = within(local.x, local.x[px], kCleanBox[0]);
|
||||
const Range box_y = within(local.y, local.y[py], kCleanBox[1]);
|
||||
const Range box_z = within(local.z, local.z[pz], kCleanBox[2]);
|
||||
for (std::size_t iy = box_y.first; iy < box_y.first + box_y.count; ++iy) {
|
||||
for (std::size_t iz = box_z.first; iz < box_z.first + box_z.count; ++iz) {
|
||||
for (std::size_t ix = box_x.first; ix < box_x.first + box_x.count; ++ix) {
|
||||
float& shown = result.display[((window_y.first + iy) * grid_nz + window_z.first + iz) * grid_nx +
|
||||
window_x.first + ix];
|
||||
shown = std::max(shown, static_cast<float>(at(iy, iz, ix) / peak));
|
||||
}
|
||||
}
|
||||
}
|
||||
result.objects.push_back({
|
||||
.index = {window_y.first + py, window_z.first + pz, window_x.first + px},
|
||||
.position = {local.x[px], local.y[py], local.z[pz]},
|
||||
.y_low = local.y[y_low],
|
||||
.y_high = local.y[y_high],
|
||||
.level = peak / (request.frame_background.median + 1e-30),
|
||||
});
|
||||
}
|
||||
std::ranges::stable_sort(result.objects, std::ranges::greater{}, &CleanObject::level);
|
||||
if (result.objects.empty()) {
|
||||
result.display.clear();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
auto marks(std::span<const CleanObject> objects, const VolumeRequest& frame) -> std::vector<Mark> {
|
||||
// Objects within SAR_MARK_GROUP_M of each other along X and Z, at any height and through
|
||||
// chains of them, are one item.
|
||||
std::vector<std::size_t> parent(objects.size());
|
||||
std::iota(parent.begin(), parent.end(), 0U);
|
||||
const auto root = [&parent](std::size_t index) {
|
||||
while (parent[index] != index) {
|
||||
index = parent[index];
|
||||
}
|
||||
return index;
|
||||
};
|
||||
for (std::size_t i = 0U; i < objects.size(); ++i) {
|
||||
for (std::size_t j = i + 1U; j < objects.size(); ++j) {
|
||||
if (std::abs(objects[i].position[0] - objects[j].position[0]) <= kMarkGroup[0] &&
|
||||
std::abs(objects[i].position[2] - objects[j].position[2]) <= kMarkGroup[1]) {
|
||||
parent[root(i)] = root(j);
|
||||
}
|
||||
}
|
||||
}
|
||||
// Members in object order, so the strongest leads each group and the groups come in the
|
||||
// order of their strongest members.
|
||||
std::vector<std::size_t> roots;
|
||||
std::vector<std::vector<std::size_t>> groups;
|
||||
for (std::size_t index = 0U; index < objects.size(); ++index) {
|
||||
const auto found = std::ranges::find(roots, root(index));
|
||||
if (found == roots.end()) {
|
||||
roots.push_back(root(index));
|
||||
groups.push_back({index});
|
||||
} else {
|
||||
groups[static_cast<std::size_t>(found - roots.begin())].push_back(index);
|
||||
}
|
||||
}
|
||||
|
||||
const auto& x = frame.grid.x;
|
||||
const auto& y = frame.grid.y;
|
||||
std::vector<Mark> result;
|
||||
for (const auto& members : groups) {
|
||||
const CleanObject& top = objects[members.front()];
|
||||
// Height of the centre: the mean height of the strong members weighted by level.
|
||||
double weight = 0.0;
|
||||
double weighted_height = 0.0;
|
||||
double top_y = -std::numeric_limits<double>::infinity();
|
||||
Mark mark;
|
||||
mark.box = {std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity(),
|
||||
std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity(),
|
||||
std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity()};
|
||||
for (const auto index : members) {
|
||||
const CleanObject& member = objects[index];
|
||||
if (member.level >= kMarkMemberRel * top.level) {
|
||||
weight += member.level;
|
||||
weighted_height += member.level * member.position[1];
|
||||
top_y = std::max(top_y, member.y_high);
|
||||
}
|
||||
auto& box = mark.box;
|
||||
box[0] = std::min(box[0], member.position[0] - kMarkPad);
|
||||
box[1] = std::max(box[1], member.position[0] + kMarkPad);
|
||||
box[2] = std::min(box[2], member.y_low);
|
||||
box[3] = std::max(box[3], member.y_high);
|
||||
box[4] = std::min(box[4], member.position[2] - kMarkPad);
|
||||
box[5] = std::max(box[5], member.position[2] + kMarkPad);
|
||||
}
|
||||
const double centre_y = weighted_height / weight;
|
||||
// The strongest point near the scene boundary may belong to a reflector partly
|
||||
// outside it, so its position and height are not yet reliable.
|
||||
const double depth = radar_yz(frame, top.position[1], top.position[2])[1];
|
||||
const bool at_edge = depth >= frame.max_depth_m - kMarkEdgeFar || depth <= frame.min_depth_m + kMarkEdgeNear ||
|
||||
top.position[0] <= x.front() + kMarkEdgeX || top.position[0] >= x.back() - kMarkEdgeX ||
|
||||
top.position[1] >= y.back() - kMarkEdgeTop;
|
||||
if (at_edge) {
|
||||
mark.kind = MarkKind::edge;
|
||||
} else if (centre_y <= kMarkLow && top_y <= kMarkLowTop) {
|
||||
mark.kind = MarkKind::low;
|
||||
} else if (centre_y <= kMarkGround) {
|
||||
mark.kind = MarkKind::ground;
|
||||
} else {
|
||||
mark.kind = MarkKind::up;
|
||||
}
|
||||
result.push_back(mark);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace radar::processing::sar3d
|
||||
@@ -0,0 +1,91 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include <vector>
|
||||
|
||||
#include "sar3d_gpu.hpp"
|
||||
|
||||
// Display stage of one 3D SAR frame (notebook Multi_depthnorm_clean.ipynb, cell 5): the
|
||||
// frame's peak over its background, CLEAN of a one-frame image into separate objects, and
|
||||
// the boxes drawn around them.
|
||||
namespace radar::processing::sar3d {
|
||||
|
||||
// BG_GATE_3D_SHOW and BG_GATE_3D_FULL: peak-to-background ratios at which a frame appears
|
||||
// and from which it is shown at full scale.
|
||||
inline constexpr double kBgGateShow = 6.0;
|
||||
inline constexpr double kBgGateFull = 8.5;
|
||||
|
||||
// SAR_CLEAN_GRID_M along X and Z, SAR_CLEAN_GRID_Y_M along Y: steps of the CLEAN search grid [m].
|
||||
inline constexpr std::array<double, 3> kCleanSearchStep{0.04, 0.10, 0.04};
|
||||
|
||||
// Background of a frame: the median over its working voxels and the maximum there
|
||||
// (frame_peak_over_background).
|
||||
struct FrameBackground {
|
||||
double median = 0.0;
|
||||
double peak = 0.0;
|
||||
|
||||
[[nodiscard]] auto ratio() const -> double { return median > 0.0 ? peak / median : 0.0; }
|
||||
};
|
||||
|
||||
// Background of `values` over the working voxels of `volume`, an image of `frame`: those it
|
||||
// reaches from inside the depth gate (non-zero) on the reference planes (sar_work_mask).
|
||||
[[nodiscard]] auto frame_background(std::span<const float> values, std::span<const float> volume,
|
||||
const VolumeRequest& frame) -> FrameBackground;
|
||||
|
||||
struct CleanRequest {
|
||||
// Request of the frame image, a one-frame aperture; CLEAN images other A-scans with it.
|
||||
VolumeRequest frame;
|
||||
std::span<const float> volume; // the frame image, unscaled
|
||||
double level = 0.0; // its normalization (sar_finish_volume)
|
||||
FrameBackground frame_background; // of that image
|
||||
// Search grid over the full scene region around the frame grid (sar_clean_search_grid).
|
||||
Axes search;
|
||||
std::span<const std::complex<float>> ascans; // full A-scan of each frame pair, back to back
|
||||
std::span<const std::complex<float>> pulse; // zero-delay point response, one A-scan long
|
||||
bool bg_gate = true; // BG_GATE_ENABLE
|
||||
};
|
||||
|
||||
struct CleanObject {
|
||||
std::array<std::size_t, 3> index{}; // peak voxel on the frame grid, [y, z, x]
|
||||
std::array<double, 3> position{}; // peak, scene X, Y, Z [m]
|
||||
double y_low = 0.0; // heights where the response stays at
|
||||
double y_high = 0.0; // _SAR_MARK_LEVEL of the peak [m]
|
||||
double level = 0.0; // peak over the frame background (level_bg)
|
||||
};
|
||||
|
||||
struct Clean {
|
||||
std::vector<CleanObject> objects; // strongest first
|
||||
// The residual in units of the frame maximum with every object over it in its own
|
||||
// 0-1 scale, on the frame grid; empty without objects.
|
||||
std::vector<float> display;
|
||||
std::size_t steps = 0U; // subtractions
|
||||
double residual_ratio = 0.0; // residual peak over its background
|
||||
};
|
||||
|
||||
// CLEAN of a one-frame image (sar_clean_frame): subtracts the strongest point responses
|
||||
// from the A-scans until the residual is background, then images every object apart.
|
||||
// Imaging leaves one of these images, not the frame, as the current volume of `gpu`.
|
||||
[[nodiscard]] auto clean(Gpu& gpu, const CleanRequest& request) -> Clean;
|
||||
|
||||
// Classes of the boxes; the values are their codes in the results.
|
||||
enum class MarkKind : std::uint8_t {
|
||||
low = 0U, // centre and top of the response near the ground (like a mine)
|
||||
ground = 1U, // on the ground
|
||||
up = 2U, // above the ground
|
||||
edge = 3U, // strongest point near the scene boundary: not yet decidable
|
||||
};
|
||||
|
||||
struct Mark {
|
||||
MarkKind kind = MarkKind::edge;
|
||||
std::array<double, 6> box{}; // X0, X1, Y0, Y1, Z0, Z1 [m]
|
||||
};
|
||||
|
||||
// Boxes around groups of the CLEAN objects of an image of `frame` (sar_mark_groups);
|
||||
// `objects` come strongest first, and so do the boxes.
|
||||
[[nodiscard]] auto marks(std::span<const CleanObject> objects, const VolumeRequest& frame) -> std::vector<Mark>;
|
||||
|
||||
} // namespace radar::processing::sar3d
|
||||
@@ -0,0 +1,733 @@
|
||||
#include "sar3d_gpu.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <bit>
|
||||
#include <climits>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#ifdef SAR3D_WITH_CUDA
|
||||
#include <cuda.h>
|
||||
#include <nvrtc.h>
|
||||
|
||||
#include <type_traits>
|
||||
#endif
|
||||
|
||||
namespace radar::processing::sar3d {
|
||||
namespace {
|
||||
|
||||
constexpr std::size_t kMaxIndexedItems = static_cast<std::size_t>(INT_MAX);
|
||||
|
||||
auto checked_product(std::size_t a, std::size_t b, std::size_t limit, const char* what) -> std::size_t {
|
||||
if (a == 0U || b == 0U || a > limit / b) {
|
||||
throw std::runtime_error(std::string("sar3d ") + what + " is too large or empty");
|
||||
}
|
||||
return a * b;
|
||||
}
|
||||
|
||||
// Two points per axis, 32-bit voxel indices and coordinates representable in float.
|
||||
void check_grid(const Axes& grid) {
|
||||
const auto plane = checked_product(grid.x.size(), grid.z.size(), kMaxIndexedItems, "grid plane");
|
||||
(void)checked_product(plane, grid.y.size(), kMaxIndexedItems, "grid (32-bit indexing; increase SAR_VOXEL_M)");
|
||||
if (grid.x.size() < 2U || grid.y.size() < 2U || grid.z.size() < 2U) {
|
||||
throw std::runtime_error("sar3d grid needs two points on every axis");
|
||||
}
|
||||
for (const auto axis : {grid.x, grid.y, grid.z}) {
|
||||
for (const double coordinate : axis) {
|
||||
if (!std::isfinite(coordinate) || std::abs(coordinate) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("sar3d grid contains an unsupported coordinate");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
auto reference_plane(std::span<const double> y, double margin, std::size_t iy) -> bool {
|
||||
if (y.size() == 2U || y.back() - y.front() <= 2.5 * margin) {
|
||||
return true;
|
||||
}
|
||||
return y[iy] >= y.front() + margin && y[iy] <= y.back() - margin;
|
||||
}
|
||||
|
||||
#ifdef SAR3D_WITH_CUDA
|
||||
namespace {
|
||||
|
||||
// Compiled at run time by NVRTC for the installed GPU, so the build needs no
|
||||
// nvcc. Arithmetic is float32 apart from a few per-voxel terms: FP64 runs at
|
||||
// 1/32-1/64 of the FP32 rate on consumer GPUs, and float32 already keeps the
|
||||
// A-scan sample position to ~1e-3 of a sample of the 8x oversampled A-scan.
|
||||
// Volumes are [y][z][x] with x fastest; their sizes stay below 2^31 (checked on
|
||||
// the host), so the hot index arithmetic is 32-bit.
|
||||
constexpr const char* kKernelSource = R"CUDA(
|
||||
struct Contribution {
|
||||
float tx[3];
|
||||
float rx[3];
|
||||
float weight;
|
||||
float inv_reference_ranges;
|
||||
};
|
||||
|
||||
// numpy.fft.ifft of a band spectrum placed at bins first_bin.. of a size-N
|
||||
// spectrum (N a power of two), summed directly: the band is a few hundred
|
||||
// bins of thousands. One thread per (pair, sample); the phase index is exact.
|
||||
extern "C" __global__ void inverse_dft(
|
||||
const float2* __restrict__ bands, const int points, const int first_bin,
|
||||
const int size, const int items, float2* __restrict__ ascans)
|
||||
{
|
||||
const unsigned index = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
if (index >= (unsigned)items) return;
|
||||
const int item = (int)index;
|
||||
const unsigned k = (unsigned)(item % size);
|
||||
const unsigned mask = (unsigned)size - 1u;
|
||||
const float2* band = bands + (item / size) * points;
|
||||
unsigned phase = (unsigned)(((unsigned long long)first_bin * k) & mask);
|
||||
const float turns_per_index = 2.0f / (float)size;
|
||||
float re = 0.0f, im = 0.0f;
|
||||
for (int n = 0; n < points; ++n) {
|
||||
float s, c;
|
||||
sincospif((float)phase * turns_per_index, &s, &c);
|
||||
const float2 b = band[n];
|
||||
re += b.x * c - b.y * s;
|
||||
im += b.x * s + b.y * c;
|
||||
phase = (phase + k) & mask;
|
||||
}
|
||||
ascans[item] = make_float2(re / (float)size, im / (float)size);
|
||||
}
|
||||
|
||||
// Coherent backprojection, one thread per voxel: |sum w*comp*h| / sum w over the
|
||||
// valid (frame, pair) terms (notebook sar_frame_contribution and the first line
|
||||
// of sar_finish_volume).
|
||||
extern "C" __global__ void backproject(
|
||||
const float2* __restrict__ ascans, const int samples,
|
||||
const Contribution* __restrict__ contributions, const int count,
|
||||
const double* __restrict__ gx, const double* __restrict__ gy, const double* __restrict__ gz,
|
||||
const int nx, const int nz, const int voxels,
|
||||
const double cos_p, const double sin_p, const double ground_depth,
|
||||
const double min_depth, const double max_depth,
|
||||
const float samples_per_metre, const int use_compensation,
|
||||
const float comp_power, const float comp_max,
|
||||
float* __restrict__ volume)
|
||||
{
|
||||
const unsigned index = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
if (index >= (unsigned)voxels) return;
|
||||
const int voxel = (int)index;
|
||||
const int row = voxel / nx;
|
||||
// Scene -> radar axes (sar_scene_to_radar_yz). The depth gate is decided in double
|
||||
// and rounded like NumPy, without a fused multiply-add: the grid corners lie on its
|
||||
// edge by construction and must be classified like in the reference.
|
||||
const double y0 = gy[row / nz] - ground_depth;
|
||||
const double zs = gz[row % nz];
|
||||
const double depth = __dadd_rn(__dmul_rn(-y0, sin_p), __dmul_rn(zs, cos_p));
|
||||
if (depth < min_depth || depth > max_depth) {
|
||||
volume[voxel] = 0.0f;
|
||||
return;
|
||||
}
|
||||
const float px = (float)gx[voxel - row * nx];
|
||||
const float py = (float)(y0 * cos_p + zs * sin_p);
|
||||
const float pz = (float)depth;
|
||||
const double last = (double)(samples - 1);
|
||||
float re = 0.0f, im = 0.0f, weights = 0.0f;
|
||||
for (int c = 0; c < count; ++c) {
|
||||
const Contribution k = contributions[c];
|
||||
const float dxt = px - k.tx[0], dyt = py - k.tx[1], dzt = pz - k.tx[2];
|
||||
const float dxr = px - k.rx[0], dyr = py - k.rx[1], dzr = pz - k.rx[2];
|
||||
const float rt = sqrtf(dxt * dxt + dyt * dyt + dzt * dzt);
|
||||
const float rr = sqrtf(dxr * dxr + dyr * dyr + dzr * dzr);
|
||||
const float position = (rt + rr) * samples_per_metre;
|
||||
if (!isfinite(position) || position < 0.0f || (double)position > last) continue;
|
||||
const int i = min((int)position, samples - 2);
|
||||
const float frac = position - (float)i;
|
||||
const float2* row_ascan = ascans + (long long)c * samples;
|
||||
const float2 a = row_ascan[i];
|
||||
const float2 b = row_ascan[i + 1];
|
||||
float w = k.weight;
|
||||
if (use_compensation) {
|
||||
// sar_compensation_weight: 1 / (geo / geo_ref)^power, clipped.
|
||||
w *= fminf(__powf(rt * rr * k.inv_reference_ranges, comp_power), comp_max);
|
||||
}
|
||||
re += w * (a.x + frac * (b.x - a.x));
|
||||
im += w * (a.y + frac * (b.y - a.y));
|
||||
weights += k.weight;
|
||||
}
|
||||
volume[voxel] = hypotf(re, im) / (weights + 1e-12f);
|
||||
}
|
||||
|
||||
// One pass of scipy.ndimage.gaussian_filter1d (mode 'reflect') along Z or X.
|
||||
extern "C" __global__ void smooth(
|
||||
const float* __restrict__ src, float* __restrict__ dst,
|
||||
const float* __restrict__ taps, const int radius,
|
||||
const int nx, const int nz, const int voxels, const int along_z)
|
||||
{
|
||||
const unsigned thread = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
if (thread >= (unsigned)voxels) return;
|
||||
const int voxel = (int)thread;
|
||||
const int length = along_z ? nz : nx;
|
||||
const int index = along_z ? (voxel / nx) % nz : voxel % nx;
|
||||
const int stride = along_z ? nx : 1;
|
||||
float sum = 0.0f;
|
||||
for (int k = -radius; k <= radius; ++k) {
|
||||
int j = index + k;
|
||||
while (j < 0 || j >= length) j = j < 0 ? -j - 1 : 2 * length - j - 1; // d c b a | a b c d | d c b a
|
||||
sum += taps[k + radius] * src[voxel + (j - index) * stride];
|
||||
}
|
||||
dst[voxel] = sum;
|
||||
}
|
||||
|
||||
// Strides select the displayed axes and the axis reduced to its maximum.
|
||||
extern "C" __global__ void max_project(
|
||||
const float* __restrict__ src, float* __restrict__ dst, const int outputs, const int inner,
|
||||
const int outer_stride, const int inner_stride, const int reduce, const int reduce_stride)
|
||||
{
|
||||
const unsigned thread = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
if (thread >= (unsigned)outputs) return;
|
||||
const int o = (int)thread;
|
||||
const float* row = src + (o / inner) * outer_stride + (o % inner) * inner_stride;
|
||||
float maximum = 0.0f;
|
||||
for (int index = 0; index < reduce; ++index) maximum = fmaxf(maximum, row[index * reduce_stride]);
|
||||
dst[o] = maximum;
|
||||
}
|
||||
|
||||
// Maximum of each Y plane and its first in-plane index, one block per plane, so the
|
||||
// host finds the first joint (y,z,x) maximum; separate projections cannot locate it.
|
||||
// A non-finite voxel makes its plane level NaN. Shared memory holds blockDim.x
|
||||
// (a power of two) levels and as many indices.
|
||||
extern "C" __global__ void peak_planes(
|
||||
const float* __restrict__ volume, const int plane, float* __restrict__ levels,
|
||||
unsigned int* __restrict__ indices)
|
||||
{
|
||||
extern __shared__ float best[];
|
||||
unsigned int* where = (unsigned int*)(best + blockDim.x);
|
||||
__shared__ int non_finite;
|
||||
if (threadIdx.x == 0) non_finite = 0;
|
||||
__syncthreads();
|
||||
const float* row = volume + blockIdx.x * plane;
|
||||
float level = 0.0f;
|
||||
unsigned int index = 0u;
|
||||
for (int i = threadIdx.x; i < plane; i += blockDim.x) {
|
||||
const float value = row[i];
|
||||
if (!isfinite(value)) non_finite = 1;
|
||||
if (value > level) { level = value; index = (unsigned int)i; }
|
||||
}
|
||||
best[threadIdx.x] = level;
|
||||
where[threadIdx.x] = index;
|
||||
__syncthreads();
|
||||
for (unsigned int stride = blockDim.x / 2; stride > 0; stride /= 2) {
|
||||
if (threadIdx.x < stride) {
|
||||
const float other = best[threadIdx.x + stride];
|
||||
const unsigned int other_index = where[threadIdx.x + stride];
|
||||
if (other > best[threadIdx.x] || (other == best[threadIdx.x] && other_index < where[threadIdx.x])) {
|
||||
best[threadIdx.x] = other;
|
||||
where[threadIdx.x] = other_index;
|
||||
}
|
||||
}
|
||||
__syncthreads();
|
||||
}
|
||||
if (threadIdx.x == 0) {
|
||||
levels[blockIdx.x] = non_finite ? __int_as_float(0x7fc00000) : best[0];
|
||||
indices[blockIdx.x] = where[0];
|
||||
}
|
||||
}
|
||||
|
||||
// Contiguous half-maximum interval through the given peak voxel on each scene axis.
|
||||
// Interpolate the two crossings; use one voxel as the resolution floor.
|
||||
extern "C" __global__ void peak_widths(
|
||||
const float* volume, const double* gx, const double* gy, const double* gz,
|
||||
const int nx, const int ny, const int nz, const int px, const int py, const int pz,
|
||||
float* widths)
|
||||
{
|
||||
const int a = (int)(blockIdx.x * blockDim.x + threadIdx.x);
|
||||
if (a >= 3) return;
|
||||
const int count[3] = {nx, ny, nz}, stride[3] = {1, nx*nz, nx}, peak[3] = {px, py, pz};
|
||||
const double* axis = a == 0 ? gx : a == 1 ? gy : gz;
|
||||
const int origin = (py*nz + pz)*nx + px, k = peak[a], step = stride[a];
|
||||
const float half = .5f * volume[origin];
|
||||
int low = k, high = k;
|
||||
while (low > 0 && volume[origin + (low-k-1)*step] >= half) --low;
|
||||
while (high+1 < count[a] && volume[origin + (high-k+1)*step] >= half) ++high;
|
||||
double left = axis[low], right = axis[high];
|
||||
if (low > 0) {
|
||||
const float inside = volume[origin + (low-k)*step], outside = volume[origin + (low-k-1)*step];
|
||||
left -= (axis[low]-axis[low-1]) * (inside-half)/(inside-outside);
|
||||
}
|
||||
if (high+1 < count[a]) {
|
||||
const float inside = volume[origin + (high-k)*step], outside = volume[origin + (high-k+1)*step];
|
||||
right += (axis[high+1]-axis[high]) * (inside-half)/(inside-outside);
|
||||
}
|
||||
widths[a] = (float)fmax(right-left, axis[1]-axis[0]);
|
||||
}
|
||||
|
||||
// Orthographic max projection. The volume is non-negative, so float bit
|
||||
// patterns order like the values and an integer atomicMax does the job. The
|
||||
// pixel is found in double, so it does not hinge on float rounding.
|
||||
extern "C" __global__ void project_oblique(
|
||||
const float* __restrict__ volume,
|
||||
const double* __restrict__ gx, const double* __restrict__ gy, const double* __restrict__ gz,
|
||||
const int nx, const int nz, const int voxels,
|
||||
const double ux, const double uy, const double uz, const double vx, const double vy, const double vz,
|
||||
const double u0, const double v0, const double inv_pixel, const int nu, const int nv,
|
||||
unsigned int* __restrict__ image)
|
||||
{
|
||||
const unsigned index = blockIdx.x * blockDim.x + threadIdx.x;
|
||||
if (index >= (unsigned)voxels) return;
|
||||
const int voxel = (int)index;
|
||||
const float value = volume[voxel];
|
||||
if (!(value > 0.0f)) return;
|
||||
const int row = voxel / nx;
|
||||
const double x = gx[voxel - row * nx];
|
||||
const double y = gy[row / nz];
|
||||
const double z = gz[row % nz];
|
||||
const int pu = __double2int_rn((ux * x + uy * y + uz * z - u0) * inv_pixel);
|
||||
const int pv = __double2int_rn((vx * x + vy * y + vz * z - v0) * inv_pixel);
|
||||
if (pu < 0 || pu >= nu || pv < 0 || pv >= nv) return;
|
||||
atomicMax(image + pv * nu + pu, __float_as_uint(value));
|
||||
}
|
||||
)CUDA";
|
||||
|
||||
constexpr unsigned kBlockSize = 256U;
|
||||
|
||||
// scipy.ndimage.gaussian_filter1d weights (truncate = 4).
|
||||
auto gaussian_taps(double sigma) -> std::vector<float> {
|
||||
if (!(sigma > 0.0) || !std::isfinite(sigma) || sigma > 100.0) {
|
||||
throw std::runtime_error("sar3d smoothing sigma must be between 0 and 100");
|
||||
}
|
||||
const int radius = static_cast<int>(4.0 * sigma + 0.5);
|
||||
std::vector<double> weights;
|
||||
double total = 0.0;
|
||||
for (int k = -radius; k <= radius; ++k) {
|
||||
weights.push_back(std::exp(-0.5 * k * k / (sigma * sigma)));
|
||||
total += weights.back();
|
||||
}
|
||||
std::vector<float> taps;
|
||||
for (const double weight : weights) {
|
||||
taps.push_back(static_cast<float>(weight / total));
|
||||
}
|
||||
return taps;
|
||||
}
|
||||
|
||||
void check(CUresult result, const char* what, bool device_required = false) {
|
||||
if (result != CUDA_SUCCESS) {
|
||||
const char* text = nullptr;
|
||||
cuGetErrorString(result, &text);
|
||||
const auto message = std::string{"CUDA "} + what + ": " + (text != nullptr ? text : "unknown error");
|
||||
if (device_required) { throw CudaUnavailable(message); }
|
||||
throw std::runtime_error(message);
|
||||
}
|
||||
}
|
||||
|
||||
void check(nvrtcResult result, const char* what) {
|
||||
if (result != NVRTC_SUCCESS) {
|
||||
throw std::runtime_error(std::string{"NVRTC "} + what + ": " + nvrtcGetErrorString(result));
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto checked_int(std::size_t value, const char* what) -> int {
|
||||
if (value > static_cast<std::size_t>(INT_MAX)) {
|
||||
throw std::runtime_error(std::string{"sar3d "} + what + " exceeds the 32-bit kernel indexing");
|
||||
}
|
||||
return static_cast<int>(value);
|
||||
}
|
||||
|
||||
// Native code when this NVRTC knows the GPU's architecture, otherwise PTX for
|
||||
// the newest architecture it knows, which the driver compiles on load.
|
||||
[[nodiscard]] auto compile_kernels(CUdevice device) -> std::string {
|
||||
int major = 0;
|
||||
int minor = 0;
|
||||
check(cuDeviceGetAttribute(&major, CU_DEVICE_ATTRIBUTE_COMPUTE_CAPABILITY_MAJOR, device), "compute capability");
|
||||
check(cuDeviceGetAttribute(&minor, CU_DEVICE_ATTRIBUTE_COMPUTE_CAPABILITY_MINOR, device), "compute capability");
|
||||
const int device_arch = 10 * major + minor;
|
||||
int arch_count = 0;
|
||||
check(nvrtcGetNumSupportedArchs(&arch_count), "supported architectures");
|
||||
std::vector<int> archs(static_cast<std::size_t>(arch_count));
|
||||
check(nvrtcGetSupportedArchs(archs.data()), "supported architectures");
|
||||
int target = 0;
|
||||
for (const int arch : archs) {
|
||||
if (arch <= device_arch) {
|
||||
target = std::max(target, arch);
|
||||
}
|
||||
}
|
||||
if (target == 0) {
|
||||
throw CudaUnavailable("CUDA GPU sm_" + std::to_string(device_arch) +
|
||||
" is older than every architecture this NVRTC supports; rebuild with NVRTC 12");
|
||||
}
|
||||
const bool native = target == device_arch;
|
||||
const std::string arch_option =
|
||||
std::string{native ? "--gpu-architecture=sm_" : "--gpu-architecture=compute_"} + std::to_string(target);
|
||||
|
||||
nvrtcProgram raw_program = nullptr;
|
||||
check(nvrtcCreateProgram(&raw_program, kKernelSource, "sar3d.cu", 0, nullptr, nullptr), "create program");
|
||||
const auto destroy = [](nvrtcProgram program) { nvrtcDestroyProgram(&program); };
|
||||
const std::unique_ptr<std::remove_pointer_t<nvrtcProgram>, decltype(destroy)> program(raw_program, destroy);
|
||||
|
||||
const char* options[] = {arch_option.c_str()};
|
||||
if (nvrtcCompileProgram(program.get(), 1, options) != NVRTC_SUCCESS) {
|
||||
std::size_t log_size = 0U;
|
||||
nvrtcGetProgramLogSize(program.get(), &log_size);
|
||||
std::string log(log_size, '\0');
|
||||
nvrtcGetProgramLog(program.get(), log.data());
|
||||
throw std::runtime_error("NVRTC failed to compile the sar3d kernels:\n" + log);
|
||||
}
|
||||
const auto image_size = native ? nvrtcGetCUBINSize : nvrtcGetPTXSize;
|
||||
const auto image_data = native ? nvrtcGetCUBIN : nvrtcGetPTX;
|
||||
std::size_t size = 0U;
|
||||
check(image_size(program.get(), &size), "kernel image size");
|
||||
std::string image(size, '\0');
|
||||
check(image_data(program.get(), image.data()), "kernel image");
|
||||
return image;
|
||||
}
|
||||
|
||||
// Device allocation that only grows, so steady-state frames never reallocate.
|
||||
class DeviceBuffer {
|
||||
public:
|
||||
DeviceBuffer() = default;
|
||||
DeviceBuffer(const DeviceBuffer&) = delete;
|
||||
auto operator=(const DeviceBuffer&) -> DeviceBuffer& = delete;
|
||||
~DeviceBuffer() {
|
||||
if (pointer_ != 0U) {
|
||||
cuMemFree(pointer_);
|
||||
}
|
||||
}
|
||||
|
||||
auto reserve(std::size_t bytes) -> CUdeviceptr {
|
||||
if (bytes > capacity_) {
|
||||
if (pointer_ != 0U) {
|
||||
check(cuMemFree(pointer_), "free");
|
||||
pointer_ = 0U;
|
||||
capacity_ = 0U;
|
||||
}
|
||||
check(cuMemAlloc(&pointer_, bytes), "allocate");
|
||||
capacity_ = bytes;
|
||||
}
|
||||
return pointer_;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
auto upload(std::span<const T> values) -> CUdeviceptr {
|
||||
reserve(values.size_bytes());
|
||||
if (!values.empty()) {
|
||||
check(cuMemcpyHtoD(pointer_, values.data(), values.size_bytes()), "upload");
|
||||
}
|
||||
return pointer_;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto pointer() const -> CUdeviceptr {
|
||||
return pointer_;
|
||||
}
|
||||
|
||||
private:
|
||||
CUdeviceptr pointer_ = 0U;
|
||||
std::size_t capacity_ = 0U;
|
||||
};
|
||||
|
||||
template <class T>
|
||||
[[nodiscard]] auto copy_to_host(CUdeviceptr source, std::size_t count) -> std::vector<T> {
|
||||
std::vector<T> values(count);
|
||||
check(cuMemcpyDtoH(values.data(), source, sizeof(T) * count), "download");
|
||||
return values;
|
||||
}
|
||||
|
||||
// `blocks` blocks of kBlockSize threads; argument types must match the kernel signature exactly.
|
||||
template <class... Args>
|
||||
void launch_blocks(CUfunction kernel, unsigned blocks, unsigned shared_bytes, Args... args) {
|
||||
void* parameters[] = {static_cast<void*>(&args)...};
|
||||
check(cuLaunchKernel(kernel, blocks, 1U, 1U, kBlockSize, 1U, 1U, shared_bytes, nullptr, parameters, nullptr),
|
||||
"launch");
|
||||
}
|
||||
|
||||
// One thread per item.
|
||||
template <class... Args>
|
||||
void launch(CUfunction kernel, int items, Args... args) {
|
||||
launch_blocks(kernel, (static_cast<unsigned>(items) + kBlockSize - 1U) / kBlockSize, 0U, args...);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct Gpu::Impl {
|
||||
// Declared first, destroyed last: buffers and the module need the context.
|
||||
struct Context {
|
||||
CUdevice device = 0;
|
||||
CUcontext handle = nullptr;
|
||||
Context() {
|
||||
check(cuInit(0U), "init", true);
|
||||
check(cuDeviceGet(&device, 0), "device", true);
|
||||
check(cuDevicePrimaryCtxRetain(&handle, device), "context", true);
|
||||
if (const CUresult result = cuCtxSetCurrent(handle); result != CUDA_SUCCESS) {
|
||||
cuDevicePrimaryCtxRelease(device);
|
||||
check(result, "context", true);
|
||||
}
|
||||
}
|
||||
~Context() { cuDevicePrimaryCtxRelease(device); }
|
||||
} context;
|
||||
struct Module {
|
||||
CUmodule handle = nullptr;
|
||||
explicit Module(const std::string& image) { check(cuModuleLoadData(&handle, image.data()), "load kernels"); }
|
||||
~Module() { cuModuleUnload(handle); }
|
||||
[[nodiscard]] auto function(const char* name) const -> CUfunction {
|
||||
CUfunction kernel = nullptr;
|
||||
check(cuModuleGetFunction(&kernel, handle, name), name);
|
||||
return kernel;
|
||||
}
|
||||
} module{compile_kernels(context.device)};
|
||||
CUfunction inverse_dft_kernel = module.function("inverse_dft");
|
||||
CUfunction backproject = module.function("backproject");
|
||||
CUfunction smooth = module.function("smooth");
|
||||
CUfunction max_project = module.function("max_project");
|
||||
CUfunction peak_planes = module.function("peak_planes");
|
||||
CUfunction peak_widths = module.function("peak_widths");
|
||||
CUfunction project_oblique = module.function("project_oblique");
|
||||
DeviceBuffer taps;
|
||||
DeviceBuffer bands;
|
||||
DeviceBuffer axes; // of the current volume: x, then y, then z
|
||||
DeviceBuffer contributions;
|
||||
DeviceBuffer ascans;
|
||||
DeviceBuffer volume; // the current volume
|
||||
DeviceBuffer scratch;
|
||||
DeviceBuffer images;
|
||||
DeviceBuffer plane_stats;
|
||||
int nx = 0; // current volume size; 0 before the first
|
||||
int ny = 0;
|
||||
int nz = 0;
|
||||
|
||||
~Impl() { cuCtxSetCurrent(context.handle); }
|
||||
|
||||
[[nodiscard]] auto gx() const -> CUdeviceptr { return axes.pointer(); }
|
||||
[[nodiscard]] auto gy() const -> CUdeviceptr { return gx() + sizeof(double) * static_cast<std::size_t>(nx); }
|
||||
[[nodiscard]] auto gz() const -> CUdeviceptr { return gy() + sizeof(double) * static_cast<std::size_t>(ny); }
|
||||
[[nodiscard]] auto voxels() const -> int { return nx * ny * nz; }
|
||||
|
||||
void use_context() const { check(cuCtxSetCurrent(context.handle), "context"); }
|
||||
|
||||
void require_volume() const {
|
||||
if (voxels() == 0) {
|
||||
throw std::runtime_error("sar3d has no current volume");
|
||||
}
|
||||
}
|
||||
|
||||
// There is no current volume until an update of it has succeeded.
|
||||
void forget_volume() { nx = ny = nz = 0; }
|
||||
|
||||
// Makes `grid` the grid of the current volume; returns the volume, sized for it.
|
||||
auto use_grid(const Axes& grid) -> CUdeviceptr {
|
||||
forget_volume();
|
||||
std::vector<double> values(grid.x.begin(), grid.x.end());
|
||||
values.insert(values.end(), grid.y.begin(), grid.y.end());
|
||||
values.insert(values.end(), grid.z.begin(), grid.z.end());
|
||||
axes.upload(std::span<const double>(values));
|
||||
const CUdeviceptr output = volume.reserve(sizeof(float) * grid.x.size() * grid.y.size() * grid.z.size());
|
||||
nx = static_cast<int>(grid.x.size());
|
||||
ny = static_cast<int>(grid.y.size());
|
||||
nz = static_cast<int>(grid.z.size());
|
||||
return output;
|
||||
}
|
||||
|
||||
// Maximum of the current volume over the reference planes of `y`, and its first voxel.
|
||||
auto plane_peak(std::span<const double> y, double margin) -> Peak {
|
||||
const auto planes = static_cast<std::size_t>(ny);
|
||||
const CUdeviceptr levels = plane_stats.reserve(2U * sizeof(float) * planes);
|
||||
const CUdeviceptr indices = levels + sizeof(float) * planes;
|
||||
launch_blocks(peak_planes, static_cast<unsigned>(ny), static_cast<unsigned>(2U * kBlockSize * sizeof(float)),
|
||||
volume.pointer(), nz * nx, levels, indices);
|
||||
const auto plane_levels = copy_to_host<float>(levels, planes);
|
||||
const auto plane_indices = copy_to_host<unsigned int>(indices, planes);
|
||||
const auto row = static_cast<unsigned int>(nx);
|
||||
Peak peak;
|
||||
for (std::size_t iy = 0U; iy < planes; ++iy) {
|
||||
if (!std::isfinite(plane_levels[iy])) {
|
||||
throw std::runtime_error("sar3d volume contains a non-finite value");
|
||||
}
|
||||
if (reference_plane(y, margin, iy) && plane_levels[iy] > peak.level) {
|
||||
peak.level = plane_levels[iy];
|
||||
peak.index = {iy, plane_indices[iy] / row, plane_indices[iy] % row};
|
||||
}
|
||||
}
|
||||
return peak;
|
||||
}
|
||||
|
||||
auto inverse_dft(
|
||||
std::span<const std::complex<float>> spectra,
|
||||
std::size_t points,
|
||||
std::size_t first_bin,
|
||||
std::size_t size
|
||||
) -> std::vector<std::complex<float>> {
|
||||
use_context();
|
||||
const std::size_t samples = spectra.size() / points * size;
|
||||
const int items = checked_int(samples, "A-scan stack");
|
||||
const CUdeviceptr input = bands.upload(spectra);
|
||||
const CUdeviceptr output = ascans.reserve(sizeof(std::complex<float>) * samples);
|
||||
launch(inverse_dft_kernel, items, input, checked_int(points, "band"), checked_int(first_bin, "first bin"),
|
||||
checked_int(size, "A-scan"), items, output);
|
||||
return copy_to_host<std::complex<float>>(output, samples);
|
||||
}
|
||||
|
||||
auto image(const VolumeRequest& request) -> Peak {
|
||||
const auto weights = gaussian_taps(request.smooth_sigma_voxels);
|
||||
use_context();
|
||||
try {
|
||||
const CUdeviceptr output = use_grid(request.grid);
|
||||
const CUdeviceptr work = scratch.reserve(sizeof(float) * static_cast<std::size_t>(voxels()));
|
||||
const CUdeviceptr terms = contributions.upload(request.contributions);
|
||||
const CUdeviceptr scans = ascans.upload(request.ascans);
|
||||
launch(backproject, voxels(), scans, checked_int(request.samples_per_ascan, "A-scan"), terms,
|
||||
checked_int(request.contributions.size(), "aperture"), gx(), gy(), gz(), nx, nz, voxels(),
|
||||
std::cos(request.pitch_rad), std::sin(request.pitch_rad), request.ground_depth_m,
|
||||
request.min_depth_m, request.max_depth_m, static_cast<float>(request.samples_per_metre),
|
||||
static_cast<int>(request.use_compensation), request.comp_range_power, request.comp_weight_max,
|
||||
output);
|
||||
const float raw_level = plane_peak(request.grid.y, request.norm_edge_margin_m).level;
|
||||
// Separable Gaussian with sigma (0, s, s) over [y][z][x]: Z first, then X, like scipy.
|
||||
const CUdeviceptr kernel = taps.upload(std::span<const float>(weights));
|
||||
const int radius = static_cast<int>(weights.size() / 2U);
|
||||
launch(smooth, voxels(), output, work, kernel, radius, nx, nz, voxels(), 1);
|
||||
launch(smooth, voxels(), work, output, kernel, radius, nx, nz, voxels(), 0);
|
||||
Peak peak = plane_peak(request.grid.y, request.norm_edge_margin_m);
|
||||
peak.raw_level = raw_level;
|
||||
return peak;
|
||||
} catch (...) {
|
||||
forget_volume();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
auto download() -> std::vector<float> {
|
||||
use_context();
|
||||
require_volume();
|
||||
return copy_to_host<float>(volume.pointer(), static_cast<std::size_t>(voxels()));
|
||||
}
|
||||
|
||||
void load(const Axes& grid, std::span<const float> values) {
|
||||
use_context();
|
||||
try {
|
||||
check(cuMemcpyHtoD(use_grid(grid), values.data(), values.size_bytes()), "upload");
|
||||
} catch (...) {
|
||||
forget_volume();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
auto project(const std::array<std::size_t, 3>& peak, const ObliqueView& view) -> Projections {
|
||||
use_context();
|
||||
require_volume();
|
||||
const auto [py, pz, px] = peak;
|
||||
if (py >= static_cast<std::size_t>(ny) || pz >= static_cast<std::size_t>(nz) ||
|
||||
px >= static_cast<std::size_t>(nx)) {
|
||||
throw std::runtime_error("sar3d peak lies outside the volume");
|
||||
}
|
||||
const int plane = nz * nx;
|
||||
const int front_size = ny * nx;
|
||||
const int side_size = ny * nz;
|
||||
const int top_size = plane;
|
||||
const int oblique_size = view.nv * view.nu;
|
||||
const auto floats = static_cast<std::size_t>(front_size) + side_size + top_size + oblique_size + 3U;
|
||||
const CUdeviceptr front = images.reserve(sizeof(float) * floats);
|
||||
const CUdeviceptr side = front + sizeof(float) * static_cast<std::size_t>(front_size);
|
||||
const CUdeviceptr top = side + sizeof(float) * static_cast<std::size_t>(side_size);
|
||||
const CUdeviceptr oblique = top + sizeof(float) * static_cast<std::size_t>(top_size);
|
||||
const CUdeviceptr widths = oblique + sizeof(float) * static_cast<std::size_t>(oblique_size);
|
||||
const CUdeviceptr values = volume.pointer();
|
||||
launch(max_project, front_size, values, front, front_size, nx, plane, 1, nz, nx);
|
||||
launch(max_project, side_size, values, side, side_size, nz, plane, nx, nx, 1);
|
||||
launch(max_project, top_size, values, top, top_size, nx, nx, 1, ny, plane);
|
||||
check(cuMemsetD32(oblique, 0U, static_cast<std::size_t>(oblique_size)), "clear");
|
||||
launch(project_oblique, voxels(), values, gx(), gy(), gz(), nx, nz, voxels(), view.u[0], view.u[1], view.u[2],
|
||||
view.v[0], view.v[1], view.v[2], view.u0, view.v0, 1.0 / view.pixel_m, view.nu, view.nv, oblique);
|
||||
launch(peak_widths, 3, values, gx(), gy(), gz(), nx, ny, nz,
|
||||
static_cast<int>(px), static_cast<int>(py), static_cast<int>(pz), widths);
|
||||
Projections result;
|
||||
result.front = copy_to_host<float>(front, static_cast<std::size_t>(front_size));
|
||||
result.side = copy_to_host<float>(side, static_cast<std::size_t>(side_size));
|
||||
result.top = copy_to_host<float>(top, static_cast<std::size_t>(top_size));
|
||||
result.oblique = copy_to_host<float>(oblique, static_cast<std::size_t>(oblique_size));
|
||||
const auto measured = copy_to_host<float>(widths, 3U);
|
||||
std::copy(measured.begin(), measured.end(), result.target_width.begin());
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
#else
|
||||
|
||||
// Built without CUDA the constructor throws, so these are never reached.
|
||||
struct Gpu::Impl {
|
||||
[[noreturn]] static void missing() { throw CudaUnavailable("SAR3D requires an NVIDIA GPU with CUDA"); }
|
||||
auto inverse_dft(std::span<const std::complex<float>>, std::size_t, std::size_t, std::size_t)
|
||||
-> std::vector<std::complex<float>> { missing(); }
|
||||
auto image(const VolumeRequest&) -> Peak { missing(); }
|
||||
auto download() -> std::vector<float> { missing(); }
|
||||
void load(const Axes&, std::span<const float>) { missing(); }
|
||||
auto project(const std::array<std::size_t, 3>&, const ObliqueView&) -> Projections { missing(); }
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Gpu::Gpu() {
|
||||
#ifdef SAR3D_WITH_CUDA
|
||||
impl_ = std::make_unique<Impl>();
|
||||
#else
|
||||
throw CudaUnavailable("SAR3D requires an NVIDIA GPU with CUDA; this program was built without "
|
||||
"nvrtc.h, cuda.h, libnvrtc or libcuda. Install CUDA and rebuild.");
|
||||
#endif
|
||||
}
|
||||
|
||||
Gpu::~Gpu() = default;
|
||||
|
||||
auto Gpu::inverse_dft(std::span<const std::complex<float>> bands, std::size_t points,
|
||||
std::size_t first_bin, std::size_t size) -> std::vector<std::complex<float>> {
|
||||
if (points == 0U || bands.empty() || bands.size() % points != 0U || !std::has_single_bit(size) ||
|
||||
size > kMaxIndexedItems || first_bin >= size || points > size - first_bin) {
|
||||
throw std::runtime_error("sar3d invalid inverse FFT dimensions");
|
||||
}
|
||||
(void)checked_product(bands.size() / points, size, kMaxIndexedItems, "A-scan stack");
|
||||
return impl_->inverse_dft(bands, points, first_bin, size);
|
||||
}
|
||||
|
||||
auto Gpu::image(const VolumeRequest& request) -> Peak {
|
||||
check_grid(request.grid);
|
||||
if (request.samples_per_ascan < 2U || request.contributions.empty() || request.samples_per_metre <= 0.0 ||
|
||||
!std::isfinite(request.samples_per_metre) || request.samples_per_metre > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("sar3d invalid aperture or A-scan dimensions");
|
||||
}
|
||||
const auto scan_count = checked_product(request.contributions.size(), request.samples_per_ascan,
|
||||
kMaxIndexedItems, "A-scan stack");
|
||||
if (request.ascans.size() != scan_count) {
|
||||
throw std::runtime_error("sar3d A-scan stack does not match the aperture");
|
||||
}
|
||||
for (const auto& term : request.contributions) {
|
||||
if (!std::isfinite(term.weight) || term.weight < 0.0F ||
|
||||
!std::isfinite(term.inv_reference_ranges) || term.inv_reference_ranges <= 0.0F) {
|
||||
throw std::runtime_error("sar3d aperture contains an invalid weight or reference range");
|
||||
}
|
||||
for (const auto point : {term.tx, term.rx}) {
|
||||
if (!std::ranges::all_of(point, [](float value) { return std::isfinite(value); })) {
|
||||
throw std::runtime_error("sar3d aperture contains a non-finite antenna position");
|
||||
}
|
||||
}
|
||||
}
|
||||
return impl_->image(request);
|
||||
}
|
||||
|
||||
auto Gpu::download() -> std::vector<float> {
|
||||
return impl_->download();
|
||||
}
|
||||
|
||||
void Gpu::load(const Axes& grid, std::span<const float> values) {
|
||||
check_grid(grid);
|
||||
if (values.size() != grid.x.size() * grid.y.size() * grid.z.size()) {
|
||||
throw std::runtime_error("sar3d volume does not match its grid");
|
||||
}
|
||||
// The oblique projection orders values by their bit patterns, which needs them non-negative.
|
||||
if (!std::ranges::all_of(values, [](float value) { return std::isfinite(value) && value >= 0.0F; })) {
|
||||
throw std::runtime_error("sar3d volume must be finite and non-negative");
|
||||
}
|
||||
impl_->load(grid, values);
|
||||
}
|
||||
|
||||
auto Gpu::project(const std::array<std::size_t, 3>& peak, const ObliqueView& view) -> Projections {
|
||||
if (view.nu <= 0 || view.nv <= 0 || !(view.pixel_m > 0.0) || !std::isfinite(view.pixel_m)) {
|
||||
throw std::runtime_error("sar3d invalid oblique view");
|
||||
}
|
||||
(void)checked_product(static_cast<std::size_t>(view.nu), static_cast<std::size_t>(view.nv),
|
||||
kMaxIndexedItems, "oblique image");
|
||||
return impl_->project(peak, view);
|
||||
}
|
||||
|
||||
} // namespace radar::processing::sar3d
|
||||
@@ -0,0 +1,125 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
// CUDA backend of the sar3d processor: A-scans of the band spectra, and the
|
||||
// backprojection of an aperture into a smoothed 3D volume. The volume stays on the
|
||||
// device for its projections and is downloaded only when its voxels are needed.
|
||||
namespace radar::processing::sar3d {
|
||||
|
||||
class CudaUnavailable : public std::runtime_error {
|
||||
public:
|
||||
using std::runtime_error::runtime_error;
|
||||
};
|
||||
|
||||
// One (aperture frame, Tx/Rx pair) term of the backprojection sum. The layout
|
||||
// is shared with the CUDA kernel.
|
||||
struct Contribution {
|
||||
std::array<float, 3> tx{}; // Tx in radar axes, shifted by the frame offset [m]
|
||||
std::array<float, 3> rx{}; // Rx in radar axes, shifted by the frame offset [m]
|
||||
float weight = 0.0F; // aperture frame weight
|
||||
float inv_reference_ranges = 0.0F; // 1 / (Rtx * Rrx) at the compensation reference point
|
||||
};
|
||||
static_assert(sizeof(Contribution) == 8U * sizeof(float));
|
||||
|
||||
// Orthographic view of the volume: a voxel at scene point p lands in pixel
|
||||
// (round((u.p - u0) / pixel), round((v.p - v0) / pixel)).
|
||||
struct ObliqueView {
|
||||
std::array<double, 3> u{}; // screen right, scene axes (X, Y, Z)
|
||||
std::array<double, 3> v{}; // screen up
|
||||
double u0 = 0.0;
|
||||
double v0 = 0.0;
|
||||
double pixel_m = 0.0;
|
||||
int nu = 0;
|
||||
int nv = 0;
|
||||
};
|
||||
|
||||
// Scene axes of a volume [m]: X across, Y height above the ground, Z along the ground.
|
||||
// Volumes are [y][z][x] with x fastest.
|
||||
struct Axes {
|
||||
std::span<const double> x;
|
||||
std::span<const double> y;
|
||||
std::span<const double> z;
|
||||
};
|
||||
|
||||
// Whether Y plane `iy` is clear of the grid's top and bottom `margin`: a constant-range
|
||||
// ridge leaves the grid there and would set a false maximum (sar_norm_reference_mask).
|
||||
[[nodiscard]] auto reference_plane(std::span<const double> y, double margin, std::size_t iy) -> bool;
|
||||
|
||||
struct VolumeRequest {
|
||||
Axes grid{};
|
||||
double pitch_rad = 0.0; // radar tilt that maps scene axes onto radar axes
|
||||
double ground_depth_m = 0.0; // height of the radar axes origin above the ground
|
||||
double min_depth_m = 0.0; // radar-depth gate; voxels outside it stay empty
|
||||
double max_depth_m = 0.0;
|
||||
std::span<const Contribution> contributions{};
|
||||
// One A-scan per contribution, back to back; sample k is the Tx+Rx path k / samples_per_metre.
|
||||
std::span<const std::complex<float>> ascans{};
|
||||
std::size_t samples_per_ascan = 0U;
|
||||
double samples_per_metre = 0.0;
|
||||
bool use_compensation = true;
|
||||
float comp_range_power = 0.0F;
|
||||
float comp_weight_max = 0.0F;
|
||||
double norm_edge_margin_m = 0.0; // SAR_NORM_EDGE_MARGIN_M of reference_plane
|
||||
double smooth_sigma_voxels = 0.0; // Gaussian over Z and X
|
||||
};
|
||||
|
||||
// Maximum of a volume over its reference planes.
|
||||
struct Peak {
|
||||
float level = 0.0F; // of the smoothed volume
|
||||
float raw_level = 0.0F; // before smoothing
|
||||
std::array<std::size_t, 3> index{}; // first voxel at `level`, [y, z, x]
|
||||
};
|
||||
|
||||
// Maximum projections of the current volume (row-major), unscaled.
|
||||
struct Projections {
|
||||
std::vector<float> front; // [y][x], maximum along Z
|
||||
std::vector<float> side; // [y][z], maximum along X
|
||||
std::vector<float> top; // [z][x], maximum along Y
|
||||
std::vector<float> oblique; // [v][u]
|
||||
std::array<float, 3> target_width{}; // contiguous FWHM through the given peak, [X, Y, Z], metres
|
||||
};
|
||||
|
||||
// CUDA context and runtime-compiled kernels. A missing device raises
|
||||
// CudaUnavailable; computation and compilation failures remain ordinary errors.
|
||||
class Gpu {
|
||||
public:
|
||||
Gpu();
|
||||
~Gpu();
|
||||
Gpu(const Gpu&) = delete;
|
||||
auto operator=(const Gpu&) -> Gpu& = delete;
|
||||
|
||||
// numpy.fft.ifft of each band spectrum placed at bins [first_bin, first_bin +
|
||||
// points) of a `size`-point spectrum; `bands` holds the bands back to back.
|
||||
[[nodiscard]] auto inverse_dft(
|
||||
std::span<const std::complex<float>> bands,
|
||||
std::size_t points,
|
||||
std::size_t first_bin,
|
||||
std::size_t size
|
||||
) -> std::vector<std::complex<float>>;
|
||||
|
||||
// Backprojects and smooths over (Z, X) into the current volume; returns its maximum over
|
||||
// the reference planes.
|
||||
auto image(const VolumeRequest& request) -> Peak;
|
||||
|
||||
// The current volume, [y][z][x].
|
||||
[[nodiscard]] auto download() -> std::vector<float>;
|
||||
|
||||
// Makes non-negative host values on `grid` the current volume.
|
||||
void load(const Axes& grid, std::span<const float> values);
|
||||
|
||||
// Max projections of the current volume and its half-maximum widths through `peak` ([y, z, x]).
|
||||
[[nodiscard]] auto project(const std::array<std::size_t, 3>& peak, const ObliqueView& view) -> Projections;
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
} // namespace radar::processing::sar3d
|
||||
@@ -0,0 +1,929 @@
|
||||
#include "sar3d_processor.hpp"
|
||||
#include "backprojection.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <bit>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <numbers>
|
||||
#include <optional>
|
||||
#include <set>
|
||||
#include <span>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "sar3d_clean.hpp"
|
||||
#include "sar3d_gpu.hpp"
|
||||
|
||||
// Port of the 3D SAR part of the reference notebook Multi_depthnorm_clean.ipynb
|
||||
// (cells 1 and 5). Comments name the notebook functions and constants each step
|
||||
// follows; the parameters keep the notebook names in the GUI.
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
|
||||
using std::numbers::pi;
|
||||
|
||||
// Notebook constants that are not live parameters.
|
||||
constexpr double kWaveSpeed = bp::kWaveSpeed; // v, eps_r = 1
|
||||
constexpr std::size_t kOversample = 8U; // BP_OVERSAMPLE
|
||||
constexpr double kCompRefDepth = 5.0; // COMP_REF_DEPTH
|
||||
constexpr float kCompRangeWeightMax = 5.0F; // COMP_RANGE_WEIGHT_MAX
|
||||
constexpr double kEdgeFrameWeight = 0.8; // SAR_EDGE_FRAME_WEIGHT
|
||||
constexpr double kSmoothSigma = 1.5; // BP_SMOOTH_SIGMA, voxels
|
||||
constexpr double kNormEdgeMargin = 0.3; // SAR_NORM_EDGE_MARGIN_M
|
||||
// Scene window: X across spans the selected antennas plus a margin on each side
|
||||
// (x_ant.min() - 2 ... x_ant.max() + 2), Y is the height above the ground
|
||||
// (SAR_Y_RANGE_M), Z along the ground follows the depth gate.
|
||||
constexpr double kSceneXMargin = 2.0;
|
||||
// The 3D view looks along the camera of the notebook's 3D scene
|
||||
// (sar_scene_layout: Plotly eye (1.7, -2.2, 0.85) in its axes X, Z, Y).
|
||||
constexpr std::array<double, 3> kCameraEyeXzy{1.7, -2.2, 0.85};
|
||||
|
||||
// Live parameters of one processing pass (notebook names).
|
||||
struct Settings {
|
||||
std::vector<std::uint32_t> inputs{}; // INPUT_IDX, ascending
|
||||
std::vector<std::uint32_t> outputs{}; // OUTPUT_IDX, ascending
|
||||
std::vector<std::array<double, 3>> rx{}; // per selected input, radar axes [m]
|
||||
std::vector<std::array<double, 3>> tx{}; // per selected output, radar axes [m]
|
||||
double f_start_hz = 0.0; // F_START
|
||||
double f_stop_hz = 0.0; // F_STOP
|
||||
double min_depth_m = 0.0; // MIN_DEPTH
|
||||
double max_depth_m = 0.0; // MAX_DEPTH
|
||||
double speed_m_s = 0.0; // SPEED_M_S
|
||||
double look_angle_rad = 0.0; // LOOK_ANGLE_DEG
|
||||
double ground_depth_m = 0.0; // BP_GROUND_DEPTH_M
|
||||
std::size_t frame_radius = 0U; // SAR_FRAME_RADIUS
|
||||
std::array<double, 3> voxel_m{}; // SAR_VOXEL_M, steps along X, Y and Z
|
||||
float comp_range_power = 0.0F; // COMP_RANGE_POWER
|
||||
bool use_compensation = true; // SAR_USE_COMPENSATION
|
||||
bool normalize_each_frame = true; // SAR_NORMALIZE_EACH_FRAME
|
||||
|
||||
bool pair_depth_equalize = true; // PAIR_DEPTH_EQUALIZE
|
||||
bool subtract_mean = true;
|
||||
std::size_t mean_count = 10U;
|
||||
double x_size_m = 0.0, z_size_m = 0.0; // 0: notebook's automatic extent
|
||||
double y_min_m = -0.5, y_max_m = 2.0; // SAR_Y_RANGE_M
|
||||
bool bg_gate_enabled = true; // BG_GATE_ENABLE
|
||||
bool clean_enabled = true; // SAR_CLEAN_ENABLE
|
||||
|
||||
// Everything the cached A-scans depend on.
|
||||
[[nodiscard]] auto ascan_key() const {
|
||||
return std::make_tuple(inputs, outputs, f_start_hz, f_stop_hz, min_depth_m, max_depth_m, speed_m_s,
|
||||
look_angle_rad, normalize_each_frame, pair_depth_equalize, subtract_mean, mean_count,
|
||||
tx, rx, x_size_m);
|
||||
}
|
||||
};
|
||||
using AscanKey = decltype(std::declval<const Settings&>().ascan_key());
|
||||
|
||||
// Requested positions that have antenna geometry, ascending; no request selects them all.
|
||||
template <class Entry, class Position>
|
||||
void select_antennas(
|
||||
const std::vector<std::uint32_t>& requested,
|
||||
const std::vector<Entry>& geometry,
|
||||
Position position_of,
|
||||
std::vector<std::uint32_t>& positions,
|
||||
std::vector<std::array<double, 3>>& coordinates
|
||||
) {
|
||||
for (const auto& entry : geometry) {
|
||||
const auto position = position_of(entry);
|
||||
if (requested.empty() || std::find(requested.begin(), requested.end(), position) != requested.end()) {
|
||||
positions.push_back(position);
|
||||
}
|
||||
}
|
||||
std::sort(positions.begin(), positions.end());
|
||||
positions.erase(std::unique(positions.begin(), positions.end()), positions.end());
|
||||
for (const auto position : positions) {
|
||||
const auto& entry = *std::find_if(geometry.begin(), geometry.end(), [&](const Entry& candidate) {
|
||||
return position_of(candidate) == position;
|
||||
});
|
||||
coordinates.push_back({entry.x_m, entry.y_m, entry.z_m});
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_settings(const config::RunConfig& run_config, const ProcessingLiveConfig& live) -> Settings {
|
||||
Settings settings{};
|
||||
select_antennas(live.gpr_input_positions, run_config.gpr.rx_geometry,
|
||||
[](const config::GprRxGeometry& entry) { return entry.input_pos; },
|
||||
settings.inputs, settings.rx);
|
||||
select_antennas(live.gpr_output_positions, run_config.gpr.tx_geometry,
|
||||
[](const config::GprTxGeometry& entry) { return entry.output_pos; },
|
||||
settings.outputs, settings.tx);
|
||||
if (settings.inputs.empty() || settings.outputs.empty()) {
|
||||
throw std::runtime_error("no selected Tx/Rx position has gpr geometry");
|
||||
}
|
||||
settings.f_start_hz = static_cast<double>(live.gpr_start_freq_mhz) * 1e6;
|
||||
settings.f_stop_hz = static_cast<double>(live.gpr_stop_freq_mhz) * 1e6;
|
||||
settings.min_depth_m = live.gpr_min_depth_m;
|
||||
settings.max_depth_m = live.gpr_max_depth_m;
|
||||
settings.speed_m_s = live.gpr_speed_m_s;
|
||||
settings.look_angle_rad = static_cast<double>(live.gpr_look_angle_deg) * (pi / 180.0);
|
||||
settings.ground_depth_m = live.sar3d_bp_ground_depth_m;
|
||||
settings.frame_radius = live.sar3d_frame_radius;
|
||||
settings.voxel_m = {live.sar3d_voxel_x_m, live.sar3d_voxel_y_m, live.sar3d_voxel_z_m};
|
||||
settings.comp_range_power = live.sar3d_comp_range_power;
|
||||
settings.use_compensation = live.sar3d_use_compensation;
|
||||
settings.normalize_each_frame = live.sar3d_normalize_each_frame;
|
||||
settings.pair_depth_equalize = live.pair_depth_equalize;
|
||||
settings.subtract_mean = live.gpr_background_subtract_enabled;
|
||||
settings.mean_count = live.gpr_background_mean_count;
|
||||
settings.x_size_m = live.sar3d_x_size_m;
|
||||
settings.z_size_m = live.sar3d_z_size_m;
|
||||
settings.y_min_m = live.sar3d_y_min_m;
|
||||
settings.y_max_m = live.sar3d_y_max_m;
|
||||
settings.bg_gate_enabled = live.sar3d_bg_gate_enabled;
|
||||
settings.clean_enabled = live.sar3d_clean_enabled;
|
||||
if (settings.x_size_m < 0.0 || settings.z_size_m < 0.0 || !(settings.y_max_m > settings.y_min_m)) {
|
||||
throw std::runtime_error("SAR scene sizes must be non-negative and SAR_Y_RANGE_M must increase");
|
||||
}
|
||||
if (!(settings.f_stop_hz > settings.f_start_hz)) {
|
||||
throw std::runtime_error("F_STOP must be above F_START");
|
||||
}
|
||||
if (!(settings.max_depth_m > settings.min_depth_m)) {
|
||||
throw std::runtime_error("MAX_DEPTH must be above MIN_DEPTH");
|
||||
}
|
||||
if (!std::ranges::all_of(settings.voxel_m, [](double step) { return step > 0.0; })) {
|
||||
throw std::runtime_error("SAR_VOXEL_M must be positive along X, Y and Z");
|
||||
}
|
||||
return settings;
|
||||
}
|
||||
|
||||
// numpy.hanning (BP_WINDOW).
|
||||
[[nodiscard]] auto hann(std::size_t count) -> std::vector<double> {
|
||||
std::vector<double> window(count);
|
||||
const auto last = static_cast<double>(count - 1U);
|
||||
for (std::size_t index = 0U; index < count; ++index) {
|
||||
window[index] = 0.5 - 0.5 * std::cos(2.0 * pi * static_cast<double>(index) / last);
|
||||
}
|
||||
return window;
|
||||
}
|
||||
|
||||
struct Band {
|
||||
std::size_t first_bin = 0U; // k0, the IFFT bin of the first band point
|
||||
std::size_t size = 0U; // IFFT length: 2(k0 + n - 1) rounded up to a power of two, x BP_OVERSAMPLE
|
||||
double df_hz = 0.0; // median frequency step
|
||||
std::vector<std::complex<float>> spectrum{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto trace_band(const ipc::SweepTraceBlock& trace, const bp::TraceClock& clock, const Settings& settings,
|
||||
const bp::Background& background)
|
||||
-> Band {
|
||||
const auto& frequency = trace.frequency_hz;
|
||||
if (trace.s21.size() != frequency.size()) {
|
||||
throw std::runtime_error("trace has different S21 and frequency lengths");
|
||||
}
|
||||
std::vector<std::size_t> points{};
|
||||
for (std::size_t point = 0U; point < frequency.size(); ++point) {
|
||||
if (!std::isfinite(frequency[point]) || frequency[point] < 0.0F ||
|
||||
(point > 0U && frequency[point] <= frequency[point - 1U])) {
|
||||
throw std::runtime_error("trace frequencies must be finite, non-negative and increasing");
|
||||
}
|
||||
if (frequency[point] >= settings.f_start_hz && frequency[point] <= settings.f_stop_hz) {
|
||||
points.push_back(point);
|
||||
}
|
||||
}
|
||||
if (points.size() < 2U) {
|
||||
throw std::runtime_error("fewer than two frequency points between F_START and F_STOP");
|
||||
}
|
||||
std::vector<double> steps{};
|
||||
for (std::size_t index = 0U; index + 1U < points.size(); ++index) {
|
||||
steps.push_back(static_cast<double>(frequency[points[index + 1U]]) - frequency[points[index]]);
|
||||
}
|
||||
Band band{};
|
||||
band.df_hz = bp::median(std::move(steps));
|
||||
if (!(band.df_hz > 0.0)) {
|
||||
throw std::runtime_error("trace frequencies do not increase");
|
||||
}
|
||||
// Bound before converting to an integer or calling bit_ceil. CUDA uses
|
||||
// 32-bit indexing; pathological sub-Hz steps must not wrap CUDA indices.
|
||||
constexpr std::size_t max_fft = 1U << 30U; // largest power of two inside CUDA's signed index
|
||||
const double first_bin = std::nearbyint(frequency[points.front()] / band.df_hz);
|
||||
if (!std::isfinite(first_bin) || first_bin < 0.0 ||
|
||||
first_bin + points.size() - 1U > max_fft / (2U * kOversample)) {
|
||||
throw std::runtime_error("frequency band exceeds the 32-bit CUDA A-scan indexing");
|
||||
}
|
||||
band.first_bin = static_cast<std::size_t>(first_bin);
|
||||
band.size = std::bit_ceil(2U * (band.first_bin + points.size() - 1U)) * kOversample;
|
||||
|
||||
// The radar moves SPEED_M_S * cos(LOOK_ANGLE_DEG) along range; its offset
|
||||
// from the collection centre at the sample time becomes the phase
|
||||
// exp(-j 4 pi f offset / v): the pair and the intra-sweep corrections in one
|
||||
// (MOTION_PHASE_SIGN = -1, direction +1).
|
||||
auto samples = bp::subtract_background(trace, background);
|
||||
bp::apply_motion_phase(samples, frequency, clock, settings.speed_m_s, settings.look_angle_rad,
|
||||
1.0, false, settings.f_start_hz, settings.f_stop_hz);
|
||||
const auto window = hann(points.size());
|
||||
for (std::size_t index = 0U; index < points.size(); ++index) {
|
||||
const auto sample = samples[points[index]];
|
||||
if (!std::isfinite(sample.real()) || !std::isfinite(sample.imag())) {
|
||||
throw std::runtime_error("S21 contains a non-finite sample");
|
||||
}
|
||||
band.spectrum.emplace_back(sample * window[index]);
|
||||
}
|
||||
return band;
|
||||
}
|
||||
|
||||
// A-scans of every selected Tx/Rx pair of one collection, (Tx, Rx) order.
|
||||
struct Frame {
|
||||
std::uint64_t center_ns = 0U;
|
||||
double dt_s = 0.0;
|
||||
std::size_t samples = 0U; // per A-scan
|
||||
std::vector<std::complex<float>> ascans{}; // [pair][sample]
|
||||
// A-scan of a point reflector at zero delay, the same band and window (sar_clean_unit_pulse).
|
||||
std::vector<std::complex<float>> pulse{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto compute_frame(
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
const Settings& settings,
|
||||
std::span<const ipc::PreprocessedCollection> previous,
|
||||
sar3d::Gpu& backend
|
||||
) -> Frame {
|
||||
const auto timing = bp::collection_timing(collection);
|
||||
const auto background = bp::background_mean(previous, settings.subtract_mean, settings.mean_count);
|
||||
std::vector<Band> bands{};
|
||||
for (const auto output : settings.outputs) {
|
||||
for (const auto input : settings.inputs) {
|
||||
const auto& traces = collection.traces;
|
||||
const auto trace = std::find_if(traces.begin(), traces.end(), [&](const auto& candidate) {
|
||||
return candidate.combo.input_pos == input && candidate.combo.output_pos == output;
|
||||
});
|
||||
if (trace == traces.end()) {
|
||||
throw std::runtime_error(
|
||||
"collection has no trace for input " + std::to_string(input) + ", output " + std::to_string(output)
|
||||
);
|
||||
}
|
||||
const auto& clock = timing.clocks[static_cast<std::size_t>(trace - traces.begin())];
|
||||
bands.push_back(trace_band(*trace, clock, settings, background));
|
||||
}
|
||||
}
|
||||
const Band& first = bands.front();
|
||||
std::vector<std::complex<float>> spectra{};
|
||||
for (const auto& band : bands) {
|
||||
if (band.first_bin != first.first_bin || band.size != first.size || band.df_hz != first.df_hz ||
|
||||
band.spectrum.size() != first.spectrum.size()) {
|
||||
throw std::runtime_error("traces of one collection have different frequency grids");
|
||||
}
|
||||
spectra.insert(spectra.end(), band.spectrum.begin(), band.spectrum.end());
|
||||
}
|
||||
// The pulse is the A-scan of a flat unit spectrum, computed after the pairs.
|
||||
for (const double weight : hann(first.spectrum.size())) {
|
||||
spectra.emplace_back(static_cast<float>(weight));
|
||||
}
|
||||
Frame frame{};
|
||||
frame.center_ns = timing.center_ns;
|
||||
frame.samples = first.size;
|
||||
frame.dt_s = 1.0 / (static_cast<double>(first.size) * first.df_hz);
|
||||
frame.ascans = backend.inverse_dft(spectra, first.spectrum.size(), first.first_bin, first.size);
|
||||
frame.pulse.assign(frame.ascans.end() - static_cast<std::ptrdiff_t>(frame.samples), frame.ascans.end());
|
||||
frame.ascans.resize(frame.ascans.size() - frame.samples);
|
||||
std::vector<std::span<std::complex<float>>> pairs;
|
||||
for (std::size_t offset = 0; offset < frame.ascans.size(); offset += frame.samples) {
|
||||
pairs.push_back(std::span(frame.ascans).subspan(offset, frame.samples));
|
||||
}
|
||||
const double dz = frame.dt_s * kWaveSpeed / 2.0;
|
||||
if (settings.pair_depth_equalize) {
|
||||
double x_min = settings.tx.front()[0], x_max = x_min, max_abs_x = 0.0;
|
||||
for (const auto* antennas : {&settings.tx, &settings.rx}) {
|
||||
for (const auto& antenna : *antennas) {
|
||||
x_min = std::min(x_min, antenna[0]);
|
||||
x_max = std::max(x_max, antenna[0]);
|
||||
max_abs_x = std::max(max_abs_x, std::abs(antenna[0]));
|
||||
}
|
||||
}
|
||||
const double half_size = settings.x_size_m > 0.0 ? settings.x_size_m / 2.0 : (x_max - x_min) / 2.0 + kSceneXMargin;
|
||||
const double center = (x_min + x_max) / 2.0;
|
||||
bp::equalize_pairs_by_depth(pairs, dz,
|
||||
bp::equalize_depth_limit(settings.max_depth_m, center - half_size, center + half_size, max_abs_x));
|
||||
}
|
||||
if (settings.normalize_each_frame) {
|
||||
for (auto pair : pairs) { bp::normalize_pair(pair, dz, settings.min_depth_m, settings.max_depth_m); }
|
||||
}
|
||||
return frame;
|
||||
}
|
||||
|
||||
// numpy.linspace(start, stop, count) with count >= 2.
|
||||
[[nodiscard]] auto linspace(double start, double stop, std::size_t count) -> std::vector<double> {
|
||||
std::vector<double> axis(count);
|
||||
const double step = (stop - start) / static_cast<double>(count - 1U);
|
||||
for (std::size_t index = 0U; index < count; ++index) {
|
||||
axis[index] = static_cast<double>(index) * step + start;
|
||||
}
|
||||
axis.back() = stop;
|
||||
return axis;
|
||||
}
|
||||
|
||||
// Every kernel uses signed 32-bit indices; `remedy` tells how to stay within them.
|
||||
constexpr std::string_view kVoxelRemedy = "increase SAR_VOXEL_M";
|
||||
|
||||
[[nodiscard]] auto checked_axis_size(double count, std::string_view remedy) -> std::size_t {
|
||||
if (!std::isfinite(count) || count < 1.0 || count > std::numeric_limits<int>::max()) {
|
||||
throw std::runtime_error("SAR axis exceeds 32-bit kernel indexing; " + std::string(remedy));
|
||||
}
|
||||
return static_cast<std::size_t>(count);
|
||||
}
|
||||
|
||||
struct Grid {
|
||||
std::vector<double> x{};
|
||||
std::vector<double> y{};
|
||||
std::vector<double> z{};
|
||||
|
||||
[[nodiscard]] auto axes() const -> sar3d::Axes {
|
||||
return {x, y, z};
|
||||
}
|
||||
};
|
||||
|
||||
// Bounds [low, high] of the scene along X, Y and Z.
|
||||
using Region = std::array<std::array<double, 2>, 3>;
|
||||
|
||||
// Scene region (sar_build_full_region_grid, 'ground' axes): Y = 0 is the ground,
|
||||
// Z runs along the ground and spans every point of the window whose radar depth
|
||||
// is inside [MIN_DEPTH, MAX_DEPTH] at some height of the window.
|
||||
[[nodiscard]] auto scene_region(const Settings& settings) -> Region {
|
||||
const double cos_p = std::cos(settings.look_angle_rad);
|
||||
const double sin_p = std::sin(settings.look_angle_rad);
|
||||
if (std::abs(cos_p) < 1e-6) {
|
||||
throw std::runtime_error("SAR grid is undefined at a radar look angle of 90 degrees");
|
||||
}
|
||||
// The pitch turns the radar about X, so antenna X is already the scene X.
|
||||
double x_min = std::numeric_limits<double>::infinity();
|
||||
double x_max = -std::numeric_limits<double>::infinity();
|
||||
for (const auto* antennas : {&settings.tx, &settings.rx}) {
|
||||
for (const auto& antenna : *antennas) {
|
||||
x_min = std::min(x_min, antenna[0] - kSceneXMargin);
|
||||
x_max = std::max(x_max, antenna[0] + kSceneXMargin);
|
||||
}
|
||||
}
|
||||
double z_min = std::numeric_limits<double>::infinity();
|
||||
double z_max = -std::numeric_limits<double>::infinity();
|
||||
for (const double height : {settings.y_min_m, settings.y_max_m}) {
|
||||
const double depth_at_zero = -(height - settings.ground_depth_m) * sin_p;
|
||||
for (const double depth : {settings.min_depth_m, settings.max_depth_m}) {
|
||||
const double z = (depth - depth_at_zero) / cos_p;
|
||||
z_min = std::min(z_min, z);
|
||||
z_max = std::max(z_max, z);
|
||||
}
|
||||
}
|
||||
return {{{x_min, x_max}, {settings.y_min_m, settings.y_max_m}, {z_min, z_max}}};
|
||||
}
|
||||
|
||||
// Region of the frame grid: the scene region with X and Z resized about their centres to
|
||||
// SAR_X_SIZE_M and SAR_Z_SIZE_M where those are set.
|
||||
[[nodiscard]] auto frame_region(Region region, const Settings& settings) -> Region {
|
||||
const auto resize = [](std::array<double, 2>& bounds, double size) {
|
||||
if (size > 0.0) {
|
||||
const double center = (bounds[0] + bounds[1]) / 2.0;
|
||||
bounds = {center - size / 2.0, center + size / 2.0};
|
||||
}
|
||||
};
|
||||
resize(region[0], settings.x_size_m);
|
||||
resize(region[2], settings.z_size_m);
|
||||
return region;
|
||||
}
|
||||
|
||||
// Region of the CLEAN search: the scene region around the frame grid (sar_clean_search_grid),
|
||||
// grown to a frame that SAR_X_SIZE_M or SAR_Z_SIZE_M widen past it.
|
||||
[[nodiscard]] auto search_region(const Region& scene, const Region& frame) -> Region {
|
||||
Region region{};
|
||||
for (std::size_t axis = 0U; axis < 3U; ++axis) {
|
||||
region[axis] = {std::min(scene[axis][0], frame[axis][0]), std::max(scene[axis][1], frame[axis][1])};
|
||||
}
|
||||
return region;
|
||||
}
|
||||
|
||||
// Grid over the region with the given steps along X, Y and Z (sar_build_full_region_grid).
|
||||
// Oversized products are rejected before the axes are allocated.
|
||||
[[nodiscard]] auto region_grid(const Region& region, const std::array<double, 3>& step, std::string_view remedy)
|
||||
-> Grid {
|
||||
std::array<std::size_t, 3> count{};
|
||||
for (std::size_t axis = 0U; axis < 3U; ++axis) {
|
||||
const auto [low, high] = region[axis];
|
||||
count[axis] = checked_axis_size(std::max(std::nearbyint((high - low) / step[axis]) + 1.0, 2.0), remedy);
|
||||
}
|
||||
if (count[0] > static_cast<std::size_t>(std::numeric_limits<int>::max()) / count[1] / count[2]) {
|
||||
throw std::runtime_error("SAR grid exceeds 32-bit kernel indexing; " + std::string(remedy));
|
||||
}
|
||||
return {linspace(region[0][0], region[0][1], count[0]), linspace(region[1][0], region[1][1], count[1]),
|
||||
linspace(region[2][0], region[2][1], count[2])};
|
||||
}
|
||||
|
||||
// Cosine taper over the aperture: 1 at the centre frame, SAR_EDGE_FRAME_WEIGHT at
|
||||
// the farthest one (sar_frame_weights).
|
||||
[[nodiscard]] auto frame_weights(const std::vector<double>& offsets_s) -> std::vector<double> {
|
||||
double farthest = 0.0;
|
||||
for (const double offset : offsets_s) {
|
||||
farthest = std::max(farthest, std::abs(offset));
|
||||
}
|
||||
std::vector<double> weights(offsets_s.size(), 1.0);
|
||||
if (farthest <= 1e-12) {
|
||||
return weights;
|
||||
}
|
||||
for (std::size_t index = 0U; index < offsets_s.size(); ++index) {
|
||||
const double r = std::min(std::abs(offsets_s[index]) / farthest, 1.0);
|
||||
weights[index] = kEdgeFrameWeight + (1.0 - kEdgeFrameWeight) * 0.5 * (1.0 + std::cos(pi * r));
|
||||
}
|
||||
return weights;
|
||||
}
|
||||
|
||||
// 1 / (Rtx * Rrx) at the compensation reference point of a pair: under its
|
||||
// midpoint, COMP_REF_DEPTH ahead, on the ground (sar_pair_geo_ref, bp_plane_y).
|
||||
[[nodiscard]] auto inverse_reference_ranges(
|
||||
const std::array<double, 3>& tx,
|
||||
const std::array<double, 3>& rx,
|
||||
const Settings& settings
|
||||
) -> float {
|
||||
const double pitch = settings.look_angle_rad;
|
||||
const std::array<double, 3> point{
|
||||
0.5 * (tx[0] + rx[0]),
|
||||
(-settings.ground_depth_m + kCompRefDepth * std::sin(pitch)) / std::cos(pitch),
|
||||
kCompRefDepth,
|
||||
};
|
||||
const auto range = [&point](const std::array<double, 3>& antenna) {
|
||||
return std::hypot(point[0] - antenna[0], point[1] - antenna[1], point[2] - antenna[2]);
|
||||
};
|
||||
return static_cast<float>(1.0 / (range(tx) * range(rx)));
|
||||
}
|
||||
|
||||
// Orthographic view along the notebook scene camera. Its axes are built in
|
||||
// Plotly's (X, Z, Y) frame, where the camera math is the familiar one, and then
|
||||
// reordered to scene (X, Y, Z).
|
||||
struct Oblique {
|
||||
sar3d::ObliqueView view{};
|
||||
std::vector<double> u{};
|
||||
std::vector<double> v{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto oblique_view(const Grid& grid, double pixel_m) -> Oblique {
|
||||
const auto& eye = kCameraEyeXzy;
|
||||
const double eye_norm = std::hypot(eye[0], eye[1], eye[2]);
|
||||
const std::array<double, 3> forward{-eye[0] / eye_norm, -eye[1] / eye_norm, -eye[2] / eye_norm};
|
||||
const double horizontal = std::hypot(forward[0], forward[1]);
|
||||
const std::array<double, 3> right{forward[1] / horizontal, -forward[0] / horizontal, 0.0}; // forward x up
|
||||
const std::array<double, 3> up{
|
||||
right[1] * forward[2] - right[2] * forward[1],
|
||||
right[2] * forward[0] - right[0] * forward[2],
|
||||
right[0] * forward[1] - right[1] * forward[0],
|
||||
};
|
||||
Oblique oblique{};
|
||||
auto& view = oblique.view;
|
||||
view.u = {right[0], right[2], right[1]};
|
||||
view.v = {up[0], up[2], up[1]};
|
||||
view.pixel_m = pixel_m;
|
||||
|
||||
double u_max = -std::numeric_limits<double>::infinity();
|
||||
double v_max = u_max;
|
||||
view.u0 = std::numeric_limits<double>::infinity();
|
||||
view.v0 = view.u0;
|
||||
for (const double x : {grid.x.front(), grid.x.back()}) {
|
||||
for (const double y : {grid.y.front(), grid.y.back()}) {
|
||||
for (const double z : {grid.z.front(), grid.z.back()}) {
|
||||
const double u = view.u[0] * x + view.u[1] * y + view.u[2] * z;
|
||||
const double v = view.v[0] * x + view.v[1] * y + view.v[2] * z;
|
||||
view.u0 = std::min(view.u0, u);
|
||||
view.v0 = std::min(view.v0, v);
|
||||
u_max = std::max(u_max, u);
|
||||
v_max = std::max(v_max, v);
|
||||
}
|
||||
}
|
||||
}
|
||||
view.nu = static_cast<int>(checked_axis_size(std::round((u_max - view.u0) / view.pixel_m) + 1.0, kVoxelRemedy));
|
||||
view.nv = static_cast<int>(checked_axis_size(std::round((v_max - view.v0) / view.pixel_m) + 1.0, kVoxelRemedy));
|
||||
if (view.nu > std::numeric_limits<int>::max() / view.nv) {
|
||||
throw std::runtime_error("SAR oblique image exceeds 32-bit kernel indexing; " + std::string(kVoxelRemedy));
|
||||
}
|
||||
for (int index = 0; index < view.nu; ++index) {
|
||||
oblique.u.push_back(view.u0 + index * view.pixel_m);
|
||||
}
|
||||
for (int index = 0; index < view.nv; ++index) {
|
||||
oblique.v.push_back(view.v0 + index * view.pixel_m);
|
||||
}
|
||||
return oblique;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_float(const std::vector<double>& values) -> std::vector<float> {
|
||||
return {values.begin(), values.end()};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_float(const std::array<double, 3>& point) -> std::array<float, 3> {
|
||||
for (const double coordinate : point) {
|
||||
if (!std::isfinite(coordinate) || std::abs(coordinate) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("SAR antenna motion exceeds the supported coordinate range");
|
||||
}
|
||||
}
|
||||
return {static_cast<float>(point[0]), static_cast<float>(point[1]), static_cast<float>(point[2])};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto distance(const std::array<double, 3>& a, const std::array<double, 3>& b) -> double {
|
||||
return std::hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto image_payload(
|
||||
std::string name,
|
||||
const std::vector<double>& x_axis,
|
||||
const std::vector<double>& y_axis,
|
||||
std::vector<float> values
|
||||
) -> ipc::ResultPayload {
|
||||
ipc::ResultPayload payload{};
|
||||
payload.processing_name = std::move(name);
|
||||
payload.kind = ipc::ResultKind::ImageF32;
|
||||
payload.image_x_axis = to_float(x_axis);
|
||||
payload.image_y_axis = to_float(y_axis);
|
||||
payload.image_values = std::move(values);
|
||||
return payload;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto table_payload(std::string name, std::uint32_t columns, std::vector<float> values)
|
||||
-> ipc::ResultPayload {
|
||||
ipc::ResultPayload payload{};
|
||||
payload.processing_name = std::move(name);
|
||||
payload.kind = ipc::ResultKind::TableF32;
|
||||
payload.table_columns = columns;
|
||||
payload.table_values = std::move(values);
|
||||
return payload;
|
||||
}
|
||||
|
||||
// CLEAN part of the status line (sar_display_note, sar_clean_report).
|
||||
[[nodiscard]] auto clean_note(const sar3d::Clean& clean) -> std::string {
|
||||
std::ostringstream note;
|
||||
note << std::fixed << std::setprecision(1) << "CLEAN: ";
|
||||
if (clean.objects.empty()) {
|
||||
note << "no objects";
|
||||
} else {
|
||||
note << clean.objects.size() << (clean.objects.size() == 1U ? " object" : " objects")
|
||||
<< ", each in its own scale";
|
||||
}
|
||||
note << " (" << clean.steps << (clean.steps == 1U ? " subtraction" : " subtractions")
|
||||
<< ", residual maximum " << clean.residual_ratio << " backgrounds)";
|
||||
return note.str();
|
||||
}
|
||||
|
||||
[[nodiscard]] auto status_payload(float code, const std::string& message) -> ipc::ResultPayload {
|
||||
std::vector<float> values{code};
|
||||
for (const unsigned char byte : message) {
|
||||
values.push_back(byte);
|
||||
}
|
||||
const auto columns = static_cast<std::uint32_t>(values.size());
|
||||
return table_payload("sar3d_status", columns, std::move(values));
|
||||
}
|
||||
|
||||
// What the views show of one frame image (sar_prepare_display).
|
||||
struct Display {
|
||||
sar3d::Projections views; // the image in units of its maximum, or the CLEAN display
|
||||
std::array<std::size_t, 3> peak{}; // marked voxel [y, z, x]: the maximum or the strongest object
|
||||
std::optional<sar3d::FrameBackground> background; // when BG_GATE_ENABLE or CLEAN needs it
|
||||
std::optional<sar3d::Clean> clean; // when CLEAN ran
|
||||
|
||||
[[nodiscard]] auto has_objects() const -> bool { return clean.has_value() && !clean->objects.empty(); }
|
||||
};
|
||||
|
||||
// Views of the frame image of `request`, the current volume of `backend` with maximum
|
||||
// `peak`. Measures the frame background when BG_GATE_ENABLE or CLEAN needs it and, when
|
||||
// the aperture is the one frame `single` (nullptr for a longer aperture, as sar_clean_frame),
|
||||
// runs CLEAN, whose objects then replace the image on the views.
|
||||
[[nodiscard]] auto display_frame(
|
||||
sar3d::Gpu& backend,
|
||||
const sar3d::VolumeRequest& request,
|
||||
const sar3d::Peak& peak,
|
||||
double normalization,
|
||||
const sar3d::ObliqueView& view,
|
||||
const Settings& settings,
|
||||
const Region& search_area,
|
||||
const Frame* single
|
||||
) -> Display {
|
||||
Display display{.views = backend.project(peak.index, view), .peak = peak.index, .background = {}, .clean = {}};
|
||||
const bool run_clean = settings.clean_enabled && single != nullptr;
|
||||
if (peak.level > 0.0F && (settings.bg_gate_enabled || run_clean)) {
|
||||
const auto volume = backend.download();
|
||||
display.background = sar3d::frame_background(volume, volume, request);
|
||||
if (run_clean) {
|
||||
// The search spans the whole scene region, so responses just outside a narrowed frame
|
||||
// are subtracted too.
|
||||
const Grid search = region_grid(search_area, sar3d::kCleanSearchStep,
|
||||
"narrow the scene or turn SAR_CLEAN_ENABLE off");
|
||||
display.clean = sar3d::clean(backend, {
|
||||
.frame = request,
|
||||
.volume = volume,
|
||||
.level = normalization,
|
||||
.frame_background = *display.background,
|
||||
.search = search.axes(),
|
||||
.ascans = single->ascans,
|
||||
.pulse = single->pulse,
|
||||
.bg_gate = settings.bg_gate_enabled,
|
||||
});
|
||||
if (display.has_objects()) {
|
||||
// CLEAN left one of its own images current; the display replaces it.
|
||||
display.peak = display.clean->objects.front().index;
|
||||
backend.load(request.grid, display.clean->display);
|
||||
display.views = backend.project(display.peak, view);
|
||||
return display;
|
||||
}
|
||||
}
|
||||
}
|
||||
auto& views = display.views;
|
||||
for (auto* image : {&views.front, &views.side, &views.top, &views.oblique}) {
|
||||
for (auto& value : *image) {
|
||||
value = static_cast<float>(static_cast<double>(value) / normalization);
|
||||
if (!std::isfinite(value)) {
|
||||
throw std::runtime_error("SAR normalization exceeds the float32 range");
|
||||
}
|
||||
}
|
||||
}
|
||||
return display;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct Sar3dProcessor::State {
|
||||
// Acquisition identity is (id, capture start): ids restart on every run,
|
||||
// while the processor and this cache outlive runs.
|
||||
using SweepKey = std::pair<std::uint64_t, std::uint64_t>;
|
||||
[[nodiscard]] static auto sweep_key(const ipc::PreprocessedCollection& collection) -> SweepKey {
|
||||
return {collection.collection_id, collection.capture_start_ns};
|
||||
}
|
||||
|
||||
struct CachedFrame {
|
||||
std::vector<SweepKey> background;
|
||||
std::shared_ptr<const Frame> value;
|
||||
std::string error;
|
||||
};
|
||||
std::map<SweepKey, CachedFrame> frames{};
|
||||
AscanKey frames_key{};
|
||||
std::unique_ptr<sar3d::Gpu> device{};
|
||||
std::exception_ptr device_error{}; // why the GPU could not be opened; not retried
|
||||
std::string last_error{};
|
||||
std::uint64_t repeated_errors = 0U;
|
||||
|
||||
auto frame(const ipc::PreprocessedCollection& collection, const Settings& settings,
|
||||
std::span<const ipc::PreprocessedCollection> previous, sar3d::Gpu& backend)
|
||||
-> std::shared_ptr<const Frame> {
|
||||
std::vector<SweepKey> background;
|
||||
if (settings.subtract_mean) {
|
||||
for (const auto& member : previous.last(std::min(previous.size(), settings.mean_count))) {
|
||||
background.push_back(sweep_key(member));
|
||||
}
|
||||
}
|
||||
if (const auto found = frames.find(sweep_key(collection)); found != frames.end() && found->second.background == background) {
|
||||
if (found->second.value == nullptr) { report_error(found->second.error); }
|
||||
return found->second.value;
|
||||
}
|
||||
std::shared_ptr<const Frame> frame{};
|
||||
std::string failure;
|
||||
try {
|
||||
frame = std::make_shared<const Frame>(compute_frame(collection, settings, previous, backend));
|
||||
} catch (const std::exception& error) {
|
||||
failure = error.what();
|
||||
report_error(failure);
|
||||
}
|
||||
frames.insert_or_assign(sweep_key(collection), CachedFrame{std::move(background), frame, std::move(failure)});
|
||||
return frame;
|
||||
}
|
||||
|
||||
auto gpu() -> sar3d::Gpu& {
|
||||
if (device == nullptr && device_error == nullptr) {
|
||||
try { device = std::make_unique<sar3d::Gpu>(); }
|
||||
catch (...) { device_error = std::current_exception(); }
|
||||
}
|
||||
if (device == nullptr) { std::rethrow_exception(device_error); }
|
||||
return *device;
|
||||
}
|
||||
|
||||
auto render(
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous,
|
||||
const Settings& settings
|
||||
) -> std::vector<ipc::ResultPayload> {
|
||||
auto& backend = gpu();
|
||||
if (settings.ascan_key() != frames_key) {
|
||||
frames.clear();
|
||||
frames_key = settings.ascan_key();
|
||||
}
|
||||
const auto recent = previous.last(std::min(previous.size(), settings.frame_radius));
|
||||
std::set<SweepKey> retained{sweep_key(collection)};
|
||||
for (const auto& member : recent) { retained.insert(sweep_key(member)); }
|
||||
// Prune even when the newest frame is invalid, so a bad input stream
|
||||
// cannot grow the failed-frame cache forever.
|
||||
std::erase_if(frames, [&](const auto& entry) { return !retained.contains(entry.first); });
|
||||
const auto newest = frame(collection, settings, previous, backend);
|
||||
if (newest == nullptr) {
|
||||
return {status_payload(2.0F, last_error)};
|
||||
}
|
||||
// SAR_CAUSAL_ONLY: current collection and r previous ones, current scene origin.
|
||||
std::vector<std::shared_ptr<const Frame>> aperture{};
|
||||
for (std::size_t i = previous.size() - recent.size(); i < previous.size(); ++i) {
|
||||
auto member_frame = frame(previous[i], settings, previous.first(i), backend);
|
||||
const bool same_axis = member_frame != nullptr && member_frame->samples == newest->samples &&
|
||||
member_frame->dt_s == newest->dt_s;
|
||||
if (same_axis) {
|
||||
aperture.push_back(std::move(member_frame));
|
||||
}
|
||||
}
|
||||
aperture.push_back(newest);
|
||||
const Frame& centre = *newest;
|
||||
|
||||
std::vector<double> offsets_s{};
|
||||
for (const auto& member : aperture) {
|
||||
// Unsigned difference reinterpreted as signed: exact for any two timestamps.
|
||||
const auto delta_ns = static_cast<std::int64_t>(member->center_ns - centre.center_ns);
|
||||
offsets_s.push_back(1e-9 * static_cast<double>(delta_ns));
|
||||
}
|
||||
const auto weights = frame_weights(offsets_s);
|
||||
const Region scene = scene_region(settings);
|
||||
const Region frame_area = frame_region(scene, settings);
|
||||
const Grid grid = region_grid(frame_area, settings.voxel_m, kVoxelRemedy);
|
||||
const double cos_p = std::cos(settings.look_angle_rad);
|
||||
const double sin_p = std::sin(settings.look_angle_rad);
|
||||
std::vector<std::array<double, 3>> corners{}; // of the scene box, in radar axes
|
||||
for (const double x : {grid.x.front(), grid.x.back()}) {
|
||||
for (const double y : {grid.y.front(), grid.y.back()}) {
|
||||
for (const double z : {grid.z.front(), grid.z.back()}) {
|
||||
const auto [y_radar, z_radar] = bp::radar_yz(y, z, settings.ground_depth_m, settings.look_angle_rad);
|
||||
corners.push_back({x, y_radar, z_radar});
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector<sar3d::Contribution> contributions{};
|
||||
double longest_path = 0.0;
|
||||
for (std::size_t index = 0U; index < aperture.size(); ++index) {
|
||||
// Horizontal travel since the centre frame, in radar axes (sar_frame_offsets).
|
||||
const double travel = settings.speed_m_s * offsets_s[index];
|
||||
const auto shifted = [&](const std::array<double, 3>& antenna) {
|
||||
return std::array<double, 3>{antenna[0], antenna[1] + travel * sin_p, antenna[2] + travel * cos_p};
|
||||
};
|
||||
for (const auto& tx : settings.tx) {
|
||||
for (const auto& rx : settings.rx) {
|
||||
const auto tx_now = shifted(tx);
|
||||
const auto rx_now = shifted(rx);
|
||||
for (const auto& corner : corners) {
|
||||
longest_path = std::max(longest_path, distance(corner, tx_now) + distance(corner, rx_now));
|
||||
}
|
||||
contributions.push_back({
|
||||
to_float(tx_now), to_float(rx_now), static_cast<float>(weights[index]),
|
||||
inverse_reference_ranges(tx, rx, settings),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
// A Tx+Rx path is convex, so the box corners bound it: samples beyond the
|
||||
// longest path are never read and are not sent to the GPU.
|
||||
const double samples_per_metre = 1.0 / (kWaveSpeed * newest->dt_s);
|
||||
const double needed = std::ceil(longest_path * samples_per_metre) + 2.0;
|
||||
const std::size_t samples = needed >= newest->samples
|
||||
? newest->samples : static_cast<std::size_t>(needed);
|
||||
std::vector<std::complex<float>> ascans{};
|
||||
for (const auto& member : aperture) {
|
||||
const std::span<const std::complex<float>> rows = member->ascans;
|
||||
for (std::size_t offset = 0U; offset < rows.size(); offset += member->samples) {
|
||||
const auto row = rows.subspan(offset, samples);
|
||||
ascans.insert(ascans.end(), row.begin(), row.end());
|
||||
}
|
||||
}
|
||||
|
||||
const sar3d::VolumeRequest request{
|
||||
.grid = grid.axes(),
|
||||
.pitch_rad = settings.look_angle_rad,
|
||||
.ground_depth_m = settings.ground_depth_m,
|
||||
.min_depth_m = settings.min_depth_m,
|
||||
.max_depth_m = settings.max_depth_m,
|
||||
.contributions = contributions,
|
||||
.ascans = ascans,
|
||||
.samples_per_ascan = samples,
|
||||
.samples_per_metre = samples_per_metre,
|
||||
.use_compensation = settings.use_compensation,
|
||||
.comp_range_power = settings.comp_range_power,
|
||||
.comp_weight_max = kCompRangeWeightMax,
|
||||
.norm_edge_margin_m = kNormEdgeMargin,
|
||||
.smooth_sigma_voxels = kSmoothSigma,
|
||||
};
|
||||
// Normalization level and maximum over the Y planes clear of the window
|
||||
// edges, where a constant-range ridge leaves the grid and would set a
|
||||
// false maximum (sar_norm_reference_mask, sar_finish_volume,
|
||||
// sar_global_max_index). The level is the volume maximum, so the
|
||||
// values at excluded Y edges can exceed 1, as in sar_finish_volume.
|
||||
const auto peak = backend.image(request);
|
||||
// The notebook normalizes before and after the linear Gaussian pass.
|
||||
// Combining both factors in double preserves its epsilons and avoids
|
||||
// float32 reciprocal overflow for a weak, nonzero signal.
|
||||
const double normalization = static_cast<double>(peak.level) + (peak.raw_level + 1e-12) * 1e-12;
|
||||
// Pixels of the largest voxel step leave no gaps between projected voxels.
|
||||
const Oblique oblique = oblique_view(grid, std::ranges::max(settings.voxel_m));
|
||||
auto display = display_frame(backend, request, peak, normalization, oblique.view, settings,
|
||||
search_region(scene, frame_area), aperture.size() == 1U ? newest.get() : nullptr);
|
||||
|
||||
std::vector<ipc::ResultPayload> payloads{};
|
||||
payloads.push_back(image_payload("sar3d_front", grid.x, grid.y, std::move(display.views.front)));
|
||||
payloads.push_back(image_payload("sar3d_side", grid.z, grid.y, std::move(display.views.side)));
|
||||
payloads.push_back(image_payload("sar3d_top", grid.x, grid.z, std::move(display.views.top)));
|
||||
payloads.push_back(image_payload("sar3d_oblique", oblique.u, oblique.v, std::move(display.views.oblique)));
|
||||
std::vector<float> basis(oblique.view.u.begin(), oblique.view.u.end());
|
||||
basis.insert(basis.end(), oblique.view.v.begin(), oblique.view.v.end());
|
||||
payloads.push_back(table_payload("sar3d_oblique_basis", 3U, std::move(basis)));
|
||||
if (!(peak.level > 0.0F)) {
|
||||
payloads.push_back(status_payload(1.0F, "No signal inside the selected depth range"));
|
||||
return payloads;
|
||||
}
|
||||
const auto [peak_y, peak_z, peak_x] = display.peak;
|
||||
payloads.push_back(table_payload("sar3d_peak", 4U, {
|
||||
static_cast<float>(normalization), static_cast<float>(grid.x[peak_x]),
|
||||
static_cast<float>(grid.y[peak_y]), static_cast<float>(grid.z[peak_z]),
|
||||
}));
|
||||
if (settings.bg_gate_enabled) {
|
||||
payloads.push_back(table_payload("sar3d_background", 3U, {
|
||||
static_cast<float>(display.background.value().ratio()),
|
||||
static_cast<float>(sar3d::kBgGateShow), static_cast<float>(sar3d::kBgGateFull),
|
||||
}));
|
||||
}
|
||||
if (display.has_objects()) {
|
||||
// The objects replace the peak marker, so its width is not published.
|
||||
std::vector<float> object_rows{};
|
||||
for (const auto& object : display.clean->objects) {
|
||||
for (const double coordinate : object.position) { object_rows.push_back(static_cast<float>(coordinate)); }
|
||||
object_rows.push_back(static_cast<float>(object.level));
|
||||
}
|
||||
payloads.push_back(table_payload("sar3d_objects", 4U, std::move(object_rows)));
|
||||
std::vector<float> mark_rows{};
|
||||
for (const auto& mark : sar3d::marks(display.clean->objects, request)) {
|
||||
mark_rows.push_back(static_cast<float>(mark.kind));
|
||||
for (const double edge : mark.box) { mark_rows.push_back(static_cast<float>(edge)); }
|
||||
}
|
||||
payloads.push_back(table_payload("sar3d_marks", 7U, std::move(mark_rows)));
|
||||
} else {
|
||||
const auto& widths = display.views.target_width;
|
||||
payloads.push_back(table_payload("sar3d_target_width", 3U, {widths[0], widths[1], widths[2]}));
|
||||
}
|
||||
std::string status = "CUDA";
|
||||
if (display.clean.has_value()) {
|
||||
status += "; " + clean_note(*display.clean);
|
||||
} else if (settings.clean_enabled) {
|
||||
status += "; CLEAN skipped: it needs a one-frame aperture, this one has " +
|
||||
std::to_string(aperture.size()) + " frames";
|
||||
}
|
||||
payloads.push_back(status_payload(0.0F, status));
|
||||
return payloads;
|
||||
}
|
||||
|
||||
void report_error(const std::string& message) {
|
||||
if (message == last_error) {
|
||||
++repeated_errors;
|
||||
return;
|
||||
}
|
||||
if (repeated_errors > 0U) {
|
||||
std::cerr << "sar3d: previous error repeated " << repeated_errors << " times\n";
|
||||
}
|
||||
std::cerr << "sar3d: " << message << '\n';
|
||||
last_error = message;
|
||||
repeated_errors = 0U;
|
||||
}
|
||||
};
|
||||
|
||||
Sar3dProcessor::Sar3dProcessor() : state_(std::make_unique<State>()) {}
|
||||
|
||||
Sar3dProcessor::~Sar3dProcessor() = default;
|
||||
|
||||
auto Sar3dProcessor::name() const -> std::string {
|
||||
return "sar3d";
|
||||
}
|
||||
|
||||
auto Sar3dProcessor::process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
try {
|
||||
results.collection_payloads =
|
||||
state_->render(collection, previous_collections, read_settings(run_config, live_config));
|
||||
// RSL3 includes the typed metadata in its fixed header.
|
||||
const auto bytes = ipc::serialize_result_collection(results).size();
|
||||
const std::size_t slot = run_config.rings.results.slot_size_bytes;
|
||||
if (bytes > slot) {
|
||||
throw std::runtime_error("the views need " + std::to_string(bytes / 1024U) +
|
||||
" KiB but a results ring slot holds " + std::to_string(slot / 1024U) +
|
||||
" KiB; increase SAR_VOXEL_M, narrow the depth range or raise "
|
||||
"rings.results.slot_size_bytes");
|
||||
}
|
||||
} catch (const sar3d::CudaUnavailable& error) {
|
||||
state_->report_error(error.what());
|
||||
results.collection_payloads = {status_payload(3.0F, error.what())};
|
||||
} catch (const std::exception& error) {
|
||||
// Keep the processing loop alive and make the reason visible in the GUI.
|
||||
state_->report_error(error.what());
|
||||
results.collection_payloads = {status_payload(2.0F, error.what())};
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,110 @@
|
||||
# Коллектор радиофотонного радара (RPH)
|
||||
|
||||
Программа сбора для АЦП L-Card E-502, которой владеет приложение. Она принимает
|
||||
двухканальный поток IQ (основной и опорный сигналы) и передаёт целые развёртки в
|
||||
пакетах `RPH1` через псевдотерминал. Поток читает и обрабатывает `RphRadarService` из
|
||||
`python_app/hardware_full/rph_radar` для процесса сбора
|
||||
`python_app.scripts.rph_radar_raw_producer`.
|
||||
|
||||
Это источник данных для `radar.model: "rph_radar"` с `radar.rph_radar.source: "e502"`.
|
||||
Источник `rfg_udp` читает UDP-поток платы RFG напрямую и эту программу не использует;
|
||||
LibreVNA работает через драйверы на Python. Как радар устроен целиком — в главе
|
||||
[Оборудование](../../docs/hardware.md).
|
||||
|
||||
## Состав
|
||||
|
||||
| Путь | Что |
|
||||
| --- | --- |
|
||||
| `src/` | коллектор (`main.cpp`) и запись в псевдотерминал |
|
||||
| `include/` | заголовок псевдотерминала и `switch_sequencer.h` |
|
||||
| `vendor/lcard/` | заголовки SDK L-Card X502/E502 (`x502api.h`, `e502api.h`, `lcard_pstdint.h`) без изменений, чтобы коллектор собирался без внешних путей |
|
||||
|
||||
Коллектор использует общий драйвер ВЧ-ключей H7992 со своим набором заголовков и
|
||||
каталогом объектов `build/rph/`.
|
||||
|
||||
## Переключение пар
|
||||
|
||||
Если ключами управляет коллектор, `SwitchSequencer` переключает ключи H7992 по
|
||||
настоящим концам развёрток (спаду DI_SYN2). Ограничение длительности и остановка
|
||||
ключи не переключают. При каждой смене пары коллектор отбрасывает оставшийся
|
||||
декодированный блок АЦП и цифрового входа и после `max(1 ms, run.settling_ms)`
|
||||
создаёт сеанс источника заново. Принадлежность развёртки новой паре по времени не
|
||||
определяется. Цикл считается законченным, только когда для каждой пары есть целая
|
||||
чистая развёртка. Если пара не меняется, сеанс сохраняется.
|
||||
|
||||
Перезапуск сеанса: остановка циклического вывода DOUT с `WAIT_DONE`, остановка и
|
||||
отключение потоков, возврат DO2 в высокий уровень, DO1 и DO8 — в низкий, закрытие и
|
||||
освобождение
|
||||
дескриптора SDK. После паузы установления создаётся, открывается и настраивается новый
|
||||
дескриптор, загружается шаблон DO1/DO8, коллектор ждёт `OutCycleSetup(WAIT_DONE)` и
|
||||
запускает АЦП, цифровой вход и вывод вместе. Накопленные отсчёты и фазовые
|
||||
накопители сбрасываются; номера развёрток и циклов идут без разрыва. Начальный
|
||||
высокий участок DI_SYN2 считается испорченным: сбор ждёт целую развёртку от фронта до
|
||||
спада. Поэтому при переоткрытии пропускается переменное число развёрток; постоянного
|
||||
числа пропусков или оценки паузы прошивки нет. Время переоткрытия, освобождение
|
||||
ресурсов USB и скорость при многих парах нужно измерить на железе.
|
||||
|
||||
## Время съёма
|
||||
|
||||
Коллектор работает от внутренних часов АЦП. АЦП и цифровой вход запускаются вместе и
|
||||
работают непрерывно; развёртки выделяет программа по линии DI_SYN2. Время отсчёта —
|
||||
монотонные часы компьютера непосредственно перед `StreamsStart` плюс номер отсчёта,
|
||||
делённый на частоту выборки. Это оценка по часам источника, а не аппаратная метка:
|
||||
задержка запуска сдвигает начало отсчёта раньше. Очереди USB и псевдотерминала время
|
||||
не сдвигают. Если оценка времени отсчёта оказалась позже момента его приёма, сбор
|
||||
останавливается, чтобы не публиковать окно съёма из будущего. Расхождение часов АЦП и
|
||||
компьютера может сработать на этой проверке при долгой работе на одной паре; её
|
||||
предел и точность окон съёма нужно измерить на железе. Данные старого сеанса SDK
|
||||
после переключения ключа чистыми не становятся ни при каком расхождении часов.
|
||||
|
||||
## Формат псевдотерминала
|
||||
|
||||
Пакет — 48-байтный заголовок в порядке байтов little-endian (`<4sIQQQQHHI`) и данные:
|
||||
`RPH1`, длина данных (u32), номер развёртки источника, номер цикла, начало и конец
|
||||
съёма в нс (четыре u64), положения входа и выхода (два u16), флаги (u32; бит 0 —
|
||||
развёртка испорчена). Без переключения коллектором оба положения равны `0xffff`, а
|
||||
номер цикла совпадает с номером развёртки. Точка данных — четыре слова u16: метка
|
||||
(`0x000a` основной сигнал, `0x00a8` опорный), номер ступени, I и Q со знаком. Кадров
|
||||
границ и пар нет. При переполнении очередь выбрасывает целые развёртки; пакет, который
|
||||
уже пишется, не обрезается. Наибольший размер данных — 1 048 544 байта. Целые циклы
|
||||
собирает Python. При переключении ключей из Python прежний коллектор и чтение
|
||||
псевдотерминала закрываются, а после паузы установления запускается новый коллектор;
|
||||
состояние фазовой обработки сохраняется.
|
||||
|
||||
## Сборка
|
||||
|
||||
```bash
|
||||
make rph_radar_collector # -> build/bin/rph_radar_collector
|
||||
```
|
||||
|
||||
В `make all` коллектор не входит; `start.sh` собирает его сам, если в профиле
|
||||
`radar.model: "rph_radar"`, кроме запуска с `--skip-build`. Сборка в Linux берёт общие
|
||||
исходники и заголовки из репозитория и линкуется с `-pthread -ldl -lutil`;
|
||||
установленный SDK L-Card для сборки не нужен.
|
||||
|
||||
## Работа
|
||||
|
||||
Закрытые библиотеки `libx502api.so` и `libe502api.so` коллектор загружает при запуске
|
||||
через `dlopen`, поэтому их должен находить загрузчик. Сервис RPH ставит
|
||||
`~/.local/lib` (их обычное место) в начало пути поиска, если профиль не задаёт
|
||||
`LD_LIBRARY_PATH` в `radar.rph_radar.env`.
|
||||
|
||||
Путь съёма один: USB, дифференциальные каналы АЦП 2 и 3, наибольшая частота от
|
||||
внутренних часов, выделение развёрток по фронту и спаду DI_SYN2, вычитание соседних
|
||||
отсчётов с разными уровнями DO1 с усреднением и наложение опорного сигнала по DO8.
|
||||
`radar.rph_radar.args` задаёт диапазон АЦП, внутреннюю опорную
|
||||
частоту, наибольшую длительность развёртки и период DO8. Остальные параметры из
|
||||
примера принимаются только как подтверждение этого пути; несовместимые режимы и
|
||||
неиспользуемые отладочные параметры отвергаются. Параметр `tty:<путь>` из
|
||||
`radar.rph_radar.tty_path` добавляет сервис.
|
||||
|
||||
Если оба ВЧ-ключа в `driver_mode: "native"`, Python передаёт коллектору
|
||||
`switching:<JSON>` с уже прочитанными настройками ключей, парами и паузой
|
||||
установления; профиль коллектор не читает. Иначе ключи переключает Python и при смене
|
||||
пары после паузы запускает новый коллектор. Съём наборов калибровки тоже идёт этим
|
||||
путём.
|
||||
|
||||
Процессом коллектора владеет запустивший его сервис; вывод коллектора уходит в журнал
|
||||
программы. В Linux смерть родителя завершает коллектор сигналом SIGKILL; обычный
|
||||
**Stop** дочитывает и закрывает собственный дочерний процесс сервиса. Завершения
|
||||
процессов по имени нет.
|
||||
@@ -0,0 +1,98 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
#include "h7992_minimal_driver.hpp"
|
||||
|
||||
namespace radar::rph {
|
||||
|
||||
// Every real GPIO transition invalidates the ADC session. No timestamp can
|
||||
// declare queued data clean; only a newly started source session can do that.
|
||||
class SwitchSequencer {
|
||||
public:
|
||||
struct Settings {
|
||||
/// Combinations to cycle through, one sweep each, in order. Empty disables
|
||||
/// the sequencer entirely (the collector then streams a single passthrough
|
||||
/// channel exactly as before).
|
||||
std::vector<radar::ipc::ComboKey> combos{};
|
||||
/// RF settling allowance after both GPIO writes complete.
|
||||
double settle_ms = 1.0;
|
||||
};
|
||||
|
||||
SwitchSequencer(
|
||||
Settings settings,
|
||||
std::unique_ptr<drivers::H7992MinimalDriver> input_switch,
|
||||
std::unique_ptr<drivers::H7992MinimalDriver> output_switch
|
||||
)
|
||||
: settings_(std::move(settings)),
|
||||
input_switch_(std::move(input_switch)),
|
||||
output_switch_(std::move(output_switch)) {}
|
||||
|
||||
[[nodiscard]] auto enabled() const -> bool { return !settings_.combos.empty(); }
|
||||
|
||||
/// Open both switch drivers and park at the first combination.
|
||||
void open() {
|
||||
if (!enabled()) {
|
||||
return;
|
||||
}
|
||||
output_switch_->open();
|
||||
input_switch_->open();
|
||||
index_ = 0;
|
||||
applied_ = settings_.combos.front();
|
||||
apply(applied_);
|
||||
upcoming_dirty_ = true;
|
||||
}
|
||||
|
||||
/// Combination the in-progress / upcoming sweep is captured under.
|
||||
[[nodiscard]] auto current_combo() const -> radar::ipc::ComboKey {
|
||||
return settings_.combos[index_];
|
||||
}
|
||||
|
||||
/// Whether the in-progress / upcoming sweep is expected to straddle a switch
|
||||
/// transition and must be dropped by the consumer.
|
||||
[[nodiscard]] auto current_dirty() const -> bool { return upcoming_dirty_; }
|
||||
|
||||
[[nodiscard]] auto cycle_id() const -> std::uint64_t { return cycle_id_; }
|
||||
|
||||
[[nodiscard]] auto settled_after_ns() const -> std::uint64_t { return settled_after_ns_; }
|
||||
void session_started() { upcoming_dirty_ = false; }
|
||||
|
||||
bool on_sweep_end(bool complete = true) {
|
||||
if (!enabled() || !complete || upcoming_dirty_) { return false; }
|
||||
index_ = (index_ + 1U) % settings_.combos.size();
|
||||
if (index_ == 0) { ++cycle_id_; }
|
||||
const radar::ipc::ComboKey next = settings_.combos[index_];
|
||||
if (!(next == applied_)) {
|
||||
apply(next);
|
||||
applied_ = next;
|
||||
upcoming_dirty_ = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private:
|
||||
void apply(const radar::ipc::ComboKey& combo) {
|
||||
// Set the transmitter before the receiver.
|
||||
output_switch_->switch_to(combo.output_pos);
|
||||
input_switch_->switch_to(combo.input_pos);
|
||||
settled_after_ns_ = static_cast<std::uint64_t>(std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()).count()) +
|
||||
static_cast<std::uint64_t>(settings_.settle_ms * 1e6);
|
||||
}
|
||||
|
||||
Settings settings_;
|
||||
std::unique_ptr<drivers::H7992MinimalDriver> input_switch_;
|
||||
std::unique_ptr<drivers::H7992MinimalDriver> output_switch_;
|
||||
std::size_t index_ = 0;
|
||||
radar::ipc::ComboKey applied_{};
|
||||
bool upcoming_dirty_ = false;
|
||||
std::uint64_t settled_after_ns_ = 0, cycle_id_ = 1;
|
||||
};
|
||||
|
||||
} // namespace radar::rph
|
||||
+7
-4
@@ -4,6 +4,7 @@
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class TtyProtocolWriter {
|
||||
public:
|
||||
@@ -21,9 +22,12 @@ public:
|
||||
TtyProtocolWriter(TtyProtocolWriter&& other) noexcept = delete;
|
||||
TtyProtocolWriter& operator=(TtyProtocolWriter&& other) noexcept = delete;
|
||||
|
||||
void emit_packet_start(uint16_t marker = 0x000A);
|
||||
void emit_step(uint16_t index, int16_t ch1_avg, int16_t ch2_avg);
|
||||
void enqueue_encoded_frames(const uint16_t* words, std::size_t frame_count);
|
||||
struct SweepMetadata {
|
||||
std::uint64_t source_id, cycle_id, capture_start_ns, capture_end_ns;
|
||||
std::uint16_t input = 0xFFFF, output = 0xFFFF;
|
||||
bool dirty = false;
|
||||
};
|
||||
void enqueue_sweep(const std::vector<uint16_t>& words, const SweepMetadata& metadata);
|
||||
StatsSnapshot stats() const;
|
||||
|
||||
const std::string& path() const;
|
||||
@@ -31,7 +35,6 @@ public:
|
||||
void shutdown();
|
||||
|
||||
private:
|
||||
void enqueue_frame(uint16_t word0, uint16_t word1, uint16_t word2, uint16_t word3);
|
||||
void worker_loop();
|
||||
|
||||
std::string path_;
|
||||
@@ -0,0 +1,530 @@
|
||||
#include "x502api.h"
|
||||
#include "e502api.h"
|
||||
#include "tty_protocol_writer.h"
|
||||
#include "switch_sequencer.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <csignal>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <dlfcn.h>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
#include <sys/prctl.h>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace {
|
||||
using Json = nlohmann::json;
|
||||
constexpr uint32_t kDiSyn2 = (1U << 13U) | (1U << 17U);
|
||||
constexpr uint32_t kDi8 = 1U << 6U; // Hardware DI8 readback offset, verified by loopback.
|
||||
constexpr uint32_t kDo8 = 1U << 7U;
|
||||
constexpr uint32_t kBlockWords = 65536;
|
||||
|
||||
[[noreturn]] void fail(const std::string& message) { throw std::runtime_error(message); }
|
||||
|
||||
void bind_to_parent() {
|
||||
const auto parent = ::getppid();
|
||||
if (::prctl(PR_SET_PDEATHSIG, SIGKILL) != 0 || ::getppid() != parent || parent == 1) {
|
||||
fail("Collector must remain owned by its launching application");
|
||||
}
|
||||
}
|
||||
|
||||
std::uint64_t monotonic_ns() {
|
||||
return static_cast<std::uint64_t>(std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()).count());
|
||||
}
|
||||
|
||||
struct Config {
|
||||
std::string tty_path;
|
||||
Json switching;
|
||||
uint32_t range = X502_ADC_RANGE_2;
|
||||
uint32_t reference_hz = X502_REF_FREQ_2000KHZ;
|
||||
uint32_t do8_period = 8;
|
||||
double duration_ms = 100;
|
||||
};
|
||||
|
||||
Config parse_args(int argc, char** argv) {
|
||||
Config cfg;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
const std::string arg = argv[i];
|
||||
const auto colon = arg.find(':');
|
||||
const auto key = arg.substr(0, colon);
|
||||
const auto value = colon == std::string::npos ? "" : arg.substr(colon + 1);
|
||||
if (key == "tty") { cfg.tty_path = value; }
|
||||
else if (key == "switching") { cfg.switching = Json::parse(value); }
|
||||
else if (key == "duration_ms") {
|
||||
std::size_t consumed = 0;
|
||||
cfg.duration_ms = std::stod(value, &consumed);
|
||||
if (consumed != value.size()) { fail("Invalid duration_ms"); }
|
||||
}
|
||||
else if (key == "do8_cycle_period") {
|
||||
std::size_t consumed = 0;
|
||||
const auto period = std::stoull(value, &consumed);
|
||||
if (consumed != value.size() || period < 4 || period > 262144 ||
|
||||
std::lcm(std::uint64_t{4}, period) > 262144) {
|
||||
fail("Combined DO1+DO8 cyclic pattern exceeds 262144 words or has an invalid period");
|
||||
}
|
||||
cfg.do8_period = static_cast<uint32_t>(period);
|
||||
} else if (key == "internal_ref_hz") {
|
||||
if (value == "2000000") { cfg.reference_hz = X502_REF_FREQ_2000KHZ; }
|
||||
else if (value == "1500000") { cfg.reference_hz = X502_REF_FREQ_1500KHZ; }
|
||||
else { fail("internal_ref_hz must be 1500000 or 2000000"); }
|
||||
} else if (key == "range") {
|
||||
const std::array<std::string, 6> ranges{"10", "5", "2", "1", "0.5", "0.2"};
|
||||
const auto found = std::find(ranges.begin(), ranges.end(), value);
|
||||
if (found == ranges.end()) { fail("Unsupported ADC range: " + value); }
|
||||
cfg.range = static_cast<uint32_t>(found - ranges.begin());
|
||||
} else if (arg != "profile:phase" && arg != "clock:internal" &&
|
||||
arg != "start:di_syn2_rise" && arg != "stop:di_syn2_fall" &&
|
||||
arg != "sample_clock_hz:max" && arg != "packet_limit:0" &&
|
||||
arg != "do1_toggle_per_frame" && arg != "do1_pair_subtract_avg" && arg != "do8_freq_ref") {
|
||||
fail("Unsupported application collector option: " + arg);
|
||||
}
|
||||
}
|
||||
if (cfg.tty_path.empty()) { fail("Application must supply tty:<path>"); }
|
||||
if (!std::isfinite(cfg.duration_ms) || cfg.duration_ms <= 0 || cfg.duration_ms > 60000) {
|
||||
fail("duration_ms must be finite and in (0, 60000]");
|
||||
}
|
||||
return cfg;
|
||||
}
|
||||
|
||||
void* open_library(const char* name) {
|
||||
auto* handle = ::dlopen(name, RTLD_NOW | RTLD_LOCAL);
|
||||
if (!handle) { fail(std::string("Cannot load ") + name + ": " + ::dlerror()); }
|
||||
return handle;
|
||||
}
|
||||
|
||||
template <typename Fn>
|
||||
Fn symbol(void* library, const char* name) {
|
||||
auto fn = reinterpret_cast<Fn>(::dlsym(library, name));
|
||||
if (!fn) { fail(std::string("Missing L-Card SDK symbol: ") + name); }
|
||||
return fn;
|
||||
}
|
||||
|
||||
struct Api {
|
||||
void* x502 = open_library("libx502api.so");
|
||||
void* e502 = open_library("libe502api.so");
|
||||
decltype(&X502_Create) Create = symbol<decltype(Create)>(x502, "X502_Create");
|
||||
decltype(&X502_Free) Free = symbol<decltype(Free)>(x502, "X502_Free");
|
||||
decltype(&X502_Close) Close = symbol<decltype(Close)>(x502, "X502_Close");
|
||||
decltype(&X502_GetErrorString) GetErrorString = symbol<decltype(GetErrorString)>(x502, "X502_GetErrorString");
|
||||
decltype(&X502_SetMode) SetMode = symbol<decltype(SetMode)>(x502, "X502_SetMode");
|
||||
decltype(&X502_StreamsStop) StreamsStop = symbol<decltype(StreamsStop)>(x502, "X502_StreamsStop");
|
||||
decltype(&X502_StreamsDisable) StreamsDisable = symbol<decltype(StreamsDisable)>(x502, "X502_StreamsDisable");
|
||||
decltype(&X502_SetSyncMode) SetSyncMode = symbol<decltype(SetSyncMode)>(x502, "X502_SetSyncMode");
|
||||
decltype(&X502_SetSyncStartMode) SetSyncStartMode = symbol<decltype(SetSyncStartMode)>(x502, "X502_SetSyncStartMode");
|
||||
decltype(&X502_SetLChannelCount) SetLChannelCount = symbol<decltype(SetLChannelCount)>(x502, "X502_SetLChannelCount");
|
||||
decltype(&X502_SetLChannel) SetLChannel = symbol<decltype(SetLChannel)>(x502, "X502_SetLChannel");
|
||||
decltype(&X502_SetAdcFreq) SetAdcFreq = symbol<decltype(SetAdcFreq)>(x502, "X502_SetAdcFreq");
|
||||
decltype(&X502_GetAdcFreq) GetAdcFreq = symbol<decltype(GetAdcFreq)>(x502, "X502_GetAdcFreq");
|
||||
decltype(&X502_SetDinFreq) SetDinFreq = symbol<decltype(SetDinFreq)>(x502, "X502_SetDinFreq");
|
||||
decltype(&X502_SetOutFreq) SetOutFreq = symbol<decltype(SetOutFreq)>(x502, "X502_SetOutFreq");
|
||||
decltype(&X502_AsyncOutDig) AsyncOutDig = symbol<decltype(AsyncOutDig)>(x502, "X502_AsyncOutDig");
|
||||
decltype(&X502_SetRefFreq) SetRefFreq = symbol<decltype(SetRefFreq)>(x502, "X502_SetRefFreq");
|
||||
decltype(&X502_SetStreamBufSize) SetStreamBufSize = symbol<decltype(SetStreamBufSize)>(x502, "X502_SetStreamBufSize");
|
||||
decltype(&X502_SetStreamStep) SetStreamStep = symbol<decltype(SetStreamStep)>(x502, "X502_SetStreamStep");
|
||||
decltype(&X502_SetDigInPullup) SetDigInPullup = symbol<decltype(SetDigInPullup)>(x502, "X502_SetDigInPullup");
|
||||
decltype(&X502_Configure) Configure = symbol<decltype(Configure)>(x502, "X502_Configure");
|
||||
decltype(&X502_StreamsEnable) StreamsEnable = symbol<decltype(StreamsEnable)>(x502, "X502_StreamsEnable");
|
||||
decltype(&X502_StreamsStart) StreamsStart = symbol<decltype(StreamsStart)>(x502, "X502_StreamsStart");
|
||||
decltype(&X502_GetRecvReadyCount) GetRecvReadyCount = symbol<decltype(GetRecvReadyCount)>(x502, "X502_GetRecvReadyCount");
|
||||
decltype(&X502_Recv) Recv = symbol<decltype(Recv)>(x502, "X502_Recv");
|
||||
decltype(&X502_OutCycleLoadStart) OutCycleLoadStart = symbol<decltype(OutCycleLoadStart)>(x502, "X502_OutCycleLoadStart");
|
||||
decltype(&X502_OutCycleSetup) OutCycleSetup = symbol<decltype(OutCycleSetup)>(x502, "X502_OutCycleSetup");
|
||||
decltype(&X502_OutCycleStop) OutCycleStop = symbol<decltype(OutCycleStop)>(x502, "X502_OutCycleStop");
|
||||
decltype(&X502_Send) Send = symbol<decltype(Send)>(x502, "X502_Send");
|
||||
decltype(&X502_PrepareData) PrepareData = symbol<decltype(PrepareData)>(x502, "X502_PrepareData");
|
||||
decltype(&X502_ProcessData) ProcessData = symbol<decltype(ProcessData)>(x502, "X502_ProcessData");
|
||||
decltype(&E502_OpenUsb) OpenUsb = symbol<decltype(OpenUsb)>(e502, "E502_OpenUsb");
|
||||
~Api() { ::dlclose(e502); ::dlclose(x502); }
|
||||
};
|
||||
|
||||
void expect_ok(const Api& api, int32_t error, const char* operation) {
|
||||
if (error != X502_ERR_OK) {
|
||||
const char* description = api.GetErrorString(error);
|
||||
fail(std::string(operation) + ": " + std::to_string(error) +
|
||||
(description ? std::string(" (") + description + ")" : ""));
|
||||
}
|
||||
}
|
||||
|
||||
struct DeviceHandle {
|
||||
const Api& api;
|
||||
t_x502_hnd hnd;
|
||||
bool opened = false;
|
||||
bool streams_started = false;
|
||||
explicit DeviceHandle(const Api& sdk) : api(sdk), hnd(api.Create()) {
|
||||
if (!hnd) { fail("X502_Create failed"); }
|
||||
}
|
||||
~DeviceHandle() {
|
||||
if (streams_started) { api.OutCycleStop(hnd, 0); api.StreamsStop(hnd); }
|
||||
if (opened) { api.Close(hnd); }
|
||||
api.Free(hnd);
|
||||
}
|
||||
};
|
||||
|
||||
volatile std::sig_atomic_t stop_requested = 0;
|
||||
void stop_handler(int signal) {
|
||||
if (stop_requested) { std::_Exit(128 + signal); }
|
||||
stop_requested = 1;
|
||||
}
|
||||
|
||||
std::unique_ptr<radar::rph::SwitchSequencer> make_switch_sequencer(const Config& cfg) {
|
||||
if (cfg.switching.is_null()) { return nullptr; }
|
||||
const auto driver = [](const Json& item) {
|
||||
if (item.value("driver", std::string("h7992")) != "h7992" || item.value("invert_logic", false)) {
|
||||
fail("Switches support only h7992 with invert_logic=false");
|
||||
}
|
||||
const auto mode = item.at("driver_mode").get<std::string>();
|
||||
if (mode != "native" && mode != "mock") { fail("Invalid switch driver_mode"); }
|
||||
return std::make_unique<radar::drivers::H7992MinimalDriver>(radar::drivers::H7992MinimalDriverSettings{
|
||||
.name = item.at("name").get<std::string>(),
|
||||
.mode = mode == "native" ? radar::config::DriverMode::Native : radar::config::DriverMode::Mock,
|
||||
.positions = item.at("positions").get<uint32_t>(),
|
||||
.default_position = item.at("default_position").get<uint32_t>(),
|
||||
.gpio_chip = item.at("gpio_chip").get<std::string>(),
|
||||
.pin_a = item.at("pin_a").get<int32_t>(), .pin_b = item.at("pin_b").get<int32_t>()});
|
||||
};
|
||||
radar::rph::SwitchSequencer::Settings settings;
|
||||
settings.settle_ms = cfg.switching.at("settling_ms").get<double>();
|
||||
if (!std::isfinite(settings.settle_ms) || settings.settle_ms < 0 || settings.settle_ms > 60000) {
|
||||
fail("Switch settling_ms must be finite and in [0, 60000]");
|
||||
}
|
||||
settings.settle_ms = std::max(1.0, settings.settle_ms);
|
||||
const auto& input = cfg.switching.at("input");
|
||||
const auto& output = cfg.switching.at("output");
|
||||
for (const auto& pair : cfg.switching.at("combos")) {
|
||||
if (!pair.is_array() || pair.size() != 2 || !pair[0].is_number_unsigned() || !pair[1].is_number_unsigned() ||
|
||||
pair[0].get<uint64_t>() >= input.at("positions").get<uint64_t>() ||
|
||||
pair[1].get<uint64_t>() >= output.at("positions").get<uint64_t>()) {
|
||||
fail("Switch combination out of range");
|
||||
}
|
||||
settings.combos.push_back({pair[0].get<uint32_t>(), pair[1].get<uint32_t>()});
|
||||
}
|
||||
if (settings.combos.empty()) { fail("Collector switching requires nonempty combos"); }
|
||||
return std::make_unique<radar::rph::SwitchSequencer>(std::move(settings), driver(input), driver(output));
|
||||
}
|
||||
|
||||
int16_t pack_raw_code_to_int16(double value) {
|
||||
const auto rounded = std::llround(value * 32767.0 / static_cast<double>(X502_ADC_SCALE_CODE_MAX));
|
||||
return static_cast<int16_t>(std::clamp<long long>(rounded, -32768, 32767));
|
||||
}
|
||||
struct Do1PairSubtractAverageState {
|
||||
struct PairSample {
|
||||
double ch1 = 0.0;
|
||||
double ch2 = 0.0;
|
||||
};
|
||||
|
||||
std::deque<PairSample> low_pairs;
|
||||
std::deque<PairSample> high_pairs;
|
||||
bool run_initialized = false;
|
||||
bool run_di1_high = false;
|
||||
double diff_sum_ch1 = 0.0;
|
||||
double diff_sum_ch2 = 0.0;
|
||||
uint32_t diff_count = 0;
|
||||
|
||||
bool pending_ch1_valid = false;
|
||||
double pending_ch1_raw = 0.0;
|
||||
bool pending_ch1_di1_high = false;
|
||||
bool pending_ch1_di2_high = false;
|
||||
bool pending_ch1_di8_high = false; // raw DI8 level (not guard-filtered)
|
||||
|
||||
uint16_t next_index = 1;
|
||||
|
||||
void clear_run_pairs() {
|
||||
low_pairs.clear();
|
||||
high_pairs.clear();
|
||||
diff_sum_ch1 = 0.0;
|
||||
diff_sum_ch2 = 0.0;
|
||||
diff_count = 0;
|
||||
}
|
||||
|
||||
void clear_pending_ch1() {
|
||||
pending_ch1_valid = false;
|
||||
pending_ch1_raw = 0.0;
|
||||
pending_ch1_di1_high = false;
|
||||
pending_ch1_di2_high = false;
|
||||
pending_ch1_di8_high = false;
|
||||
}
|
||||
|
||||
void reset_packet() {
|
||||
clear_run_pairs();
|
||||
run_initialized = false;
|
||||
run_di1_high = false;
|
||||
clear_pending_ch1();
|
||||
next_index = 1;
|
||||
}
|
||||
|
||||
void start_run(bool di1_high) {
|
||||
clear_run_pairs();
|
||||
run_initialized = true;
|
||||
run_di1_high = di1_high;
|
||||
}
|
||||
|
||||
void clear_run() {
|
||||
clear_run_pairs();
|
||||
run_initialized = false;
|
||||
}
|
||||
|
||||
void add_pending_ch1(double raw_code, bool di1_high, bool di2_high,
|
||||
bool di8_high_level = false) {
|
||||
pending_ch1_valid = true;
|
||||
pending_ch1_raw = raw_code;
|
||||
pending_ch1_di1_high = di1_high;
|
||||
pending_ch1_di2_high = di2_high;
|
||||
pending_ch1_di8_high = di8_high_level;
|
||||
}
|
||||
|
||||
void add_pair_sample(bool di2_high, double ch1_value, double ch2_value) {
|
||||
const PairSample sample{ch1_value, ch2_value};
|
||||
if (di2_high) {
|
||||
high_pairs.push_back(sample);
|
||||
} else {
|
||||
low_pairs.push_back(sample);
|
||||
}
|
||||
|
||||
while (!low_pairs.empty() && !high_pairs.empty()) {
|
||||
const PairSample low = low_pairs.front();
|
||||
const PairSample high = high_pairs.front();
|
||||
low_pairs.pop_front();
|
||||
high_pairs.pop_front();
|
||||
|
||||
diff_sum_ch1 += (low.ch1 - high.ch1);
|
||||
diff_sum_ch2 += (low.ch2 - high.ch2);
|
||||
++diff_count;
|
||||
}
|
||||
}
|
||||
|
||||
bool has_output() const { return diff_count != 0U; }
|
||||
|
||||
double avg_diff_ch1() const {
|
||||
if (diff_count == 0U) {
|
||||
return 0.0;
|
||||
}
|
||||
return diff_sum_ch1 / static_cast<double>(diff_count);
|
||||
}
|
||||
|
||||
double avg_diff_ch2() const {
|
||||
if (diff_count == 0U) {
|
||||
return 0.0;
|
||||
}
|
||||
return diff_sum_ch2 / static_cast<double>(diff_count);
|
||||
}
|
||||
};
|
||||
|
||||
int run(const Config& cfg) {
|
||||
stop_requested = 0;
|
||||
struct sigaction action{};
|
||||
action.sa_handler = stop_handler;
|
||||
sigemptyset(&action.sa_mask);
|
||||
if (::sigaction(SIGINT, &action, nullptr) != 0 || ::sigaction(SIGTERM, &action, nullptr) != 0) {
|
||||
fail("Cannot install collector stop handlers");
|
||||
}
|
||||
Api api;
|
||||
auto sequencer = make_switch_sequencer(cfg);
|
||||
if (sequencer) { sequencer->open(); }
|
||||
std::unique_ptr<DeviceHandle> device;
|
||||
bool restart_requested = true;
|
||||
double adc_hz = 0;
|
||||
uint64_t epoch_ns = 0, max_frames = 0;
|
||||
TtyProtocolWriter writer(cfg.tty_path, 2U * (48U + 65534U * 16U));
|
||||
std::vector<uint32_t> raw(kBlockWords), din(kBlockWords);
|
||||
std::vector<double> adc(kBlockWords);
|
||||
std::deque<double> pending_adc;
|
||||
std::deque<uint32_t> pending_din;
|
||||
std::vector<uint16_t> words;
|
||||
Do1PairSubtractAverageState pairs;
|
||||
std::array<double, 2> reference_sum{};
|
||||
uint32_t reference_count = 0;
|
||||
bool active = false, previous_gate = false, initialized = false;
|
||||
uint64_t sample_index = 0, sweep_id = 0, frames = 0;
|
||||
TtyProtocolWriter::SweepMetadata metadata{};
|
||||
const auto append = [&](uint16_t marker, uint16_t step, double i, double q) {
|
||||
words.insert(words.end(), {marker, step, static_cast<uint16_t>(pack_raw_code_to_int16(i)),
|
||||
static_cast<uint16_t>(pack_raw_code_to_int16(q))});
|
||||
};
|
||||
const auto finish_step = [&]() {
|
||||
if (pairs.run_initialized && pairs.has_output()) {
|
||||
if (pairs.next_index == 0xffff) { fail("Sweep exceeds 65534 phase points"); }
|
||||
const auto step = pairs.next_index++;
|
||||
append(0x000a, step, pairs.avg_diff_ch1(), pairs.avg_diff_ch2());
|
||||
if (reference_count) {
|
||||
append(0x00a8, step, reference_sum[0] / reference_count, reference_sum[1] / reference_count);
|
||||
}
|
||||
}
|
||||
pairs.clear_run();
|
||||
reference_sum = {};
|
||||
reference_count = 0;
|
||||
};
|
||||
const auto finish_sweep = [&](uint64_t end_ns, bool complete) {
|
||||
finish_step();
|
||||
metadata.capture_end_ns = end_ns;
|
||||
metadata.dirty = metadata.dirty || !complete;
|
||||
if (!words.empty() && end_ns > metadata.capture_start_ns) { writer.enqueue_sweep(words, metadata); }
|
||||
if (sequencer && frames && !metadata.dirty) { restart_requested = sequencer->on_sweep_end(complete); }
|
||||
words.clear();
|
||||
active = false;
|
||||
};
|
||||
const auto stop_session = [&]() {
|
||||
if (!device) { return; }
|
||||
expect_ok(api, api.OutCycleStop(device->hnd, X502_OUT_CYCLE_FLAGS_WAIT_DONE), "Stop DOUT cycle");
|
||||
expect_ok(api, api.StreamsStop(device->hnd), "Stop streams");
|
||||
device->streams_started = false;
|
||||
expect_ok(api, api.StreamsDisable(device->hnd, X502_STREAM_ALL_IN | X502_STREAM_ALL_OUT), "Disable streams");
|
||||
expect_ok(api, api.AsyncOutDig(device->hnd, 2, ~(3U | kDo8)), "Restore DO2 high");
|
||||
expect_ok(api, api.Close(device->hnd), "Close ADC session");
|
||||
device->opened = false;
|
||||
device.reset(); // X502_Free destroys this session's decoder/DMA buffers.
|
||||
};
|
||||
const auto start_session = [&]() {
|
||||
stop_session();
|
||||
pending_adc.clear(); pending_din.clear(); words.clear();
|
||||
pairs.reset_packet(); reference_sum = {}; reference_count = 0;
|
||||
active = false; previous_gate = false; initialized = false;
|
||||
sample_index = 0; frames = 0;
|
||||
while (sequencer && monotonic_ns() < sequencer->settled_after_ns() && !stop_requested) { ::usleep(1000); }
|
||||
if (stop_requested) { return; }
|
||||
device = std::make_unique<DeviceHandle>(api);
|
||||
expect_ok(api, api.OpenUsb(device->hnd, nullptr), "Open E-502");
|
||||
device->opened = true;
|
||||
expect_ok(api, api.SetMode(device->hnd, X502_MODE_FPGA), "Set FPGA mode");
|
||||
expect_ok(api, api.StreamsStop(device->hnd), "Stop prior streams");
|
||||
expect_ok(api, api.StreamsDisable(device->hnd, X502_STREAM_ALL_IN | X502_STREAM_ALL_OUT), "Disable prior streams");
|
||||
expect_ok(api, api.SetSyncMode(device->hnd, X502_SYNC_INTERNAL), "Set internal sample clock");
|
||||
expect_ok(api, api.SetSyncStartMode(device->hnd, X502_SYNC_INTERNAL), "Set immediate start");
|
||||
expect_ok(api, api.SetRefFreq(device->hnd, cfg.reference_hz), "Set reference clock");
|
||||
expect_ok(api, api.SetLChannelCount(device->hnd, 2), "Set logical channels");
|
||||
for (uint32_t ch = 0; ch < 2; ++ch) {
|
||||
expect_ok(api, api.SetLChannel(device->hnd, ch, ch + 2, X502_LCH_MODE_DIFF, cfg.range, 1), "Set ADC channel");
|
||||
}
|
||||
adc_hz = cfg.reference_hz;
|
||||
double frame_hz = 0;
|
||||
expect_ok(api, api.SetAdcFreq(device->hnd, &adc_hz, nullptr), "Set maximum ADC frequency");
|
||||
expect_ok(api, api.GetAdcFreq(device->hnd, &adc_hz, &frame_hz), "Get ADC frequencies");
|
||||
double din_hz = adc_hz, dout_hz = adc_hz;
|
||||
expect_ok(api, api.SetDinFreq(device->hnd, &din_hz), "Set DIN frequency");
|
||||
expect_ok(api, api.SetOutFreq(device->hnd, &dout_hz), "Set DOUT frequency");
|
||||
if (!std::isfinite(adc_hz) || adc_hz <= 0 || !std::isfinite(frame_hz) || frame_hz <= 0 ||
|
||||
!std::isfinite(din_hz) || !std::isfinite(dout_hz) || adc_hz > cfg.reference_hz ||
|
||||
std::abs(frame_hz * 2 - adc_hz) > 0.5 || std::abs(din_hz - adc_hz) > 0.5 || std::abs(dout_hz - adc_hz) > 0.5) {
|
||||
fail("ADC, DIN and DOUT must have the same finite sample frequency");
|
||||
}
|
||||
for (const auto stream : {X502_STREAM_CH_IN, X502_STREAM_CH_OUT}) {
|
||||
expect_ok(api, api.SetStreamBufSize(device->hnd, stream, 16U * 1024U * 1024U), "Set stream buffer");
|
||||
expect_ok(api, api.SetStreamStep(device->hnd, stream, kBlockWords), "Set stream step");
|
||||
}
|
||||
expect_ok(api, api.SetDigInPullup(device->hnd, 0), "Set input pulls");
|
||||
expect_ok(api, api.Configure(device->hnd, 0), "Configure E-502");
|
||||
expect_ok(api, api.AsyncOutDig(device->hnd, 2, 0), "Set DO2 high");
|
||||
expect_ok(api, api.StreamsEnable(device->hnd, X502_STREAM_ADC | X502_STREAM_DIN | X502_STREAM_DOUT), "Enable streams");
|
||||
|
||||
// Parse bounds were checked before loading the SDK or allocating either vector.
|
||||
const auto pattern_size = std::lcm(uint32_t{4}, cfg.do8_period);
|
||||
std::vector<uint32_t> pattern(pattern_size), encoded(pattern_size);
|
||||
for (std::size_t i = 0; i < pattern.size(); ++i) {
|
||||
pattern[i] = 2U | (i % 4U >= 2U ? 1U : 0U) |
|
||||
(i % cfg.do8_period >= cfg.do8_period - 2U ? kDo8 : 0U);
|
||||
}
|
||||
expect_ok(api, api.OutCycleLoadStart(device->hnd, pattern_size), "Allocate DOUT cycle");
|
||||
expect_ok(api, api.PrepareData(device->hnd, nullptr, nullptr, pattern.data(), pattern_size, 0, encoded.data()), "Encode DOUT cycle");
|
||||
for (std::size_t sent = 0; sent < encoded.size();) {
|
||||
const auto n = api.Send(device->hnd, encoded.data() + sent, static_cast<uint32_t>(encoded.size() - sent), 50);
|
||||
if (n <= 0) { fail("Send DOUT cycle failed or timed out: " + std::to_string(n)); }
|
||||
sent += static_cast<std::size_t>(n);
|
||||
}
|
||||
expect_ok(api, api.OutCycleSetup(device->hnd, X502_OUT_CYCLE_FLAGS_WAIT_DONE), "Activate DOUT cycle");
|
||||
max_frames = std::max<uint64_t>(1, static_cast<uint64_t>(std::llround(cfg.duration_ms * frame_hz / 1000.0)));
|
||||
// Diagnostic source-clock estimate only. Freshness follows the new SDK
|
||||
// session after settled GPIO, independently of either clock's drift.
|
||||
epoch_ns = monotonic_ns();
|
||||
expect_ok(api, api.StreamsStart(device->hnd), "Start ADC/DIN/DOUT streams");
|
||||
device->streams_started = true;
|
||||
if (sequencer) { sequencer->session_started(); }
|
||||
restart_requested = false;
|
||||
};
|
||||
while (!stop_requested) {
|
||||
if (restart_requested) { start_session(); }
|
||||
if (stop_requested) { break; }
|
||||
writer.throw_if_failed();
|
||||
uint32_t ready = 0;
|
||||
expect_ok(api, api.GetRecvReadyCount(device->hnd, &ready), "Read ADC backlog");
|
||||
const auto count = api.Recv(device->hnd, raw.data(), ready ? std::min(ready, kBlockWords) : kBlockWords, ready ? 0 : 50);
|
||||
if (count < 0) { expect_ok(api, count, "Receive ADC/DIN"); }
|
||||
if (count == 0) { continue; }
|
||||
uint32_t adc_count = kBlockWords, din_count = kBlockWords;
|
||||
expect_ok(api, api.ProcessData(device->hnd, raw.data(), static_cast<uint32_t>(count), 0,
|
||||
adc.data(), &adc_count, din.data(), &din_count), "Decode ADC/DIN");
|
||||
pending_adc.insert(pending_adc.end(), adc.begin(), adc.begin() + adc_count);
|
||||
pending_din.insert(pending_din.end(), din.begin(), din.begin() + din_count);
|
||||
if (pending_adc.size() > 1'000'000 || pending_din.size() > 1'000'000) {
|
||||
fail("ADC/DIN alignment backlog exceeded its bound");
|
||||
}
|
||||
const auto received_at_ns = monotonic_ns();
|
||||
while (!pending_adc.empty() && !pending_din.empty() && !stop_requested && !restart_requested) {
|
||||
const auto value = pending_adc.front(); pending_adc.pop_front();
|
||||
const auto bits = pending_din.front(); pending_din.pop_front();
|
||||
const auto at_ns = epoch_ns + static_cast<uint64_t>(static_cast<long double>(sample_index) * 1e9L / adc_hz);
|
||||
if (at_ns > received_at_ns) { fail("ADC source clock leads host monotonic clock; restart acquisition"); }
|
||||
const auto channel = sample_index++ % 2U;
|
||||
const bool gate = (bits & kDiSyn2) != 0;
|
||||
if (!initialized || (!previous_gate && gate)) {
|
||||
if (gate) {
|
||||
pairs.reset_packet(); reference_sum = {}; reference_count = 0;
|
||||
words.clear(); frames = 0; ++sweep_id;
|
||||
metadata = {sweep_id, sequencer ? sequencer->cycle_id() : sweep_id, at_ns, at_ns,
|
||||
0xffff, 0xffff, !initialized};
|
||||
if (sequencer) {
|
||||
const auto combo = sequencer->current_combo();
|
||||
metadata.input = static_cast<uint16_t>(combo.input_pos);
|
||||
metadata.output = static_cast<uint16_t>(combo.output_pos);
|
||||
metadata.dirty = metadata.dirty || sequencer->current_dirty();
|
||||
}
|
||||
active = true;
|
||||
}
|
||||
initialized = true;
|
||||
}
|
||||
if (active && previous_gate && !gate) { finish_sweep(at_ns, true); }
|
||||
previous_gate = gate;
|
||||
if (!active) { continue; }
|
||||
const bool di1 = (bits & 1U) != 0, di2 = (bits & 2U) != 0, di8 = (bits & kDi8) != 0;
|
||||
if (channel == 0) {
|
||||
pairs.add_pending_ch1(value, di1, di2, di8);
|
||||
} else {
|
||||
if (pairs.pending_ch1_valid && pairs.pending_ch1_di1_high == di1 && pairs.pending_ch1_di2_high == di2) {
|
||||
if (pairs.pending_ch1_di8_high && di8) {
|
||||
reference_sum[0] += pairs.pending_ch1_raw;
|
||||
reference_sum[1] += value;
|
||||
++reference_count;
|
||||
} else if (!pairs.pending_ch1_di8_high && !di8) {
|
||||
if (!pairs.run_initialized) { pairs.start_run(di1); }
|
||||
else if (pairs.run_di1_high != di1) { finish_step(); pairs.start_run(di1); }
|
||||
pairs.add_pair_sample(di2, pairs.pending_ch1_raw, value);
|
||||
}
|
||||
}
|
||||
pairs.clear_pending_ch1();
|
||||
if (++frames >= max_frames) { finish_sweep(at_ns, false); }
|
||||
}
|
||||
}
|
||||
}
|
||||
// Stop discards the incomplete current sweep. It never advances the switches.
|
||||
writer.shutdown();
|
||||
stop_session();
|
||||
return 0;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
try {
|
||||
bind_to_parent();
|
||||
return run(parse_args(argc, argv));
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << "Collector error: " << error.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,317 @@
|
||||
#include "tty_protocol_writer.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <deque>
|
||||
#include <exception>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <cerrno>
|
||||
#include <condition_variable>
|
||||
#include <fcntl.h>
|
||||
#include <limits.h>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <pty.h>
|
||||
#include <sstream>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace {
|
||||
|
||||
std::string io_error(const std::string& action, const std::string& path) {
|
||||
std::ostringstream out;
|
||||
out << action << " '" << path << "': " << std::strerror(errno);
|
||||
return out.str();
|
||||
}
|
||||
|
||||
void close_fd_if_open(int& fd) noexcept {
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void set_fd_raw(int fd) {
|
||||
struct termios tio {};
|
||||
if (::tcgetattr(fd, &tio) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot read tty attributes for", std::to_string(fd)));
|
||||
}
|
||||
::cfmakeraw(&tio);
|
||||
tio.c_cc[VMIN] = 1;
|
||||
tio.c_cc[VTIME] = 0;
|
||||
if (::tcsetattr(fd, TCSANOW, &tio) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot apply raw tty attributes to", std::to_string(fd)));
|
||||
}
|
||||
}
|
||||
|
||||
std::optional<std::string> read_link_target(const std::string& path) {
|
||||
std::array<char, PATH_MAX> buf {};
|
||||
const ssize_t len = ::readlink(path.c_str(), buf.data(), buf.size() - 1U);
|
||||
if (len < 0) {
|
||||
if (errno == EINVAL || errno == ENOENT) {
|
||||
return std::nullopt;
|
||||
}
|
||||
throw std::runtime_error(io_error("Cannot read symlink", path));
|
||||
}
|
||||
buf[static_cast<std::size_t>(len)] = '\0';
|
||||
return std::string(buf.data());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct TtyProtocolWriter::Impl {
|
||||
explicit Impl(std::size_t bytes) : capacity_bytes(bytes) {}
|
||||
|
||||
int fd = -1;
|
||||
int slave_fd = -1;
|
||||
std::string slave_path;
|
||||
bool owns_link = false;
|
||||
const std::size_t capacity_bytes;
|
||||
std::deque<std::vector<std::uint8_t>> packets;
|
||||
std::size_t queued_bytes = 0;
|
||||
|
||||
mutable std::mutex mutex;
|
||||
std::condition_variable data_ready_cv;
|
||||
std::thread worker;
|
||||
bool stop_requested = false;
|
||||
std::exception_ptr failure;
|
||||
StatsSnapshot stats;
|
||||
};
|
||||
|
||||
TtyProtocolWriter::TtyProtocolWriter(std::string path, std::size_t ring_capacity_bytes)
|
||||
: path_(std::move(path)),
|
||||
impl_(std::make_unique<Impl>(ring_capacity_bytes)) {
|
||||
std::array<char, PATH_MAX> slave_name {};
|
||||
if (::openpty(&impl_->fd, &impl_->slave_fd, slave_name.data(), nullptr, nullptr) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot create PTY bridge for", path_));
|
||||
}
|
||||
|
||||
try {
|
||||
impl_->slave_path = slave_name.data();
|
||||
set_fd_raw(impl_->slave_fd);
|
||||
|
||||
struct stat st {};
|
||||
if (::lstat(path_.c_str(), &st) == 0) {
|
||||
if (!S_ISLNK(st.st_mode) && !S_ISREG(st.st_mode)) {
|
||||
throw std::runtime_error("Refusing to replace non-link path '" + path_ + "'");
|
||||
}
|
||||
if (::unlink(path_.c_str()) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot remove existing tty link", path_));
|
||||
}
|
||||
} else if (errno != ENOENT) {
|
||||
throw std::runtime_error(io_error("Cannot inspect tty link path", path_));
|
||||
}
|
||||
|
||||
if (::symlink(impl_->slave_path.c_str(), path_.c_str()) != 0) {
|
||||
throw std::runtime_error(io_error("Cannot create tty symlink", path_));
|
||||
}
|
||||
impl_->owns_link = true;
|
||||
} catch (...) {
|
||||
close_fd_if_open(impl_->slave_fd);
|
||||
close_fd_if_open(impl_->fd);
|
||||
throw;
|
||||
}
|
||||
|
||||
try {
|
||||
const auto flags = ::fcntl(impl_->fd, F_GETFL);
|
||||
if (flags < 0 || ::fcntl(impl_->fd, F_SETFL, flags | O_NONBLOCK) < 0) {
|
||||
throw std::runtime_error(io_error("Cannot make tty output nonblocking", path_));
|
||||
}
|
||||
impl_->worker = std::thread([this]() { worker_loop(); });
|
||||
} catch (...) {
|
||||
if (impl_->owns_link) {
|
||||
::unlink(path_.c_str());
|
||||
}
|
||||
close_fd_if_open(impl_->slave_fd);
|
||||
close_fd_if_open(impl_->fd);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
TtyProtocolWriter::~TtyProtocolWriter() {
|
||||
try {
|
||||
shutdown();
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (impl_->owns_link && !path_.empty()) {
|
||||
try {
|
||||
const auto target = read_link_target(path_);
|
||||
if (target && (*target == impl_->slave_path)) {
|
||||
::unlink(path_.c_str());
|
||||
}
|
||||
} catch (...) {
|
||||
}
|
||||
impl_->owns_link = false;
|
||||
}
|
||||
|
||||
close_fd_if_open(impl_->slave_fd);
|
||||
close_fd_if_open(impl_->fd);
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::enqueue_sweep(const std::vector<uint16_t>& words, const SweepMetadata& metadata) {
|
||||
if (words.empty()) { return; }
|
||||
if (words.size() % 4U != 0U || words.size() > 65534U * 8U ||
|
||||
metadata.source_id == 0 || metadata.cycle_id == 0 ||
|
||||
metadata.capture_start_ns == 0 || metadata.capture_end_ns <= metadata.capture_start_ns) {
|
||||
throw std::runtime_error("Invalid RPH sweep packet");
|
||||
}
|
||||
std::vector<std::uint8_t> packet{'R', 'P', 'H', '1'};
|
||||
packet.reserve(48U + words.size() * 2U);
|
||||
const auto append = [&](std::uint64_t value, unsigned bytes) {
|
||||
for (unsigned i = 0; i < bytes; ++i) { packet.push_back(static_cast<std::uint8_t>(value >> (i * 8U))); }
|
||||
};
|
||||
append(words.size() * 2U, 4);
|
||||
append(metadata.source_id, 8);
|
||||
append(metadata.cycle_id, 8);
|
||||
append(metadata.capture_start_ns, 8);
|
||||
append(metadata.capture_end_ns, 8);
|
||||
append(metadata.input, 2);
|
||||
append(metadata.output, 2);
|
||||
append(metadata.dirty, 4);
|
||||
for (const auto word : words) { append(word, 2); }
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (impl_->failure) { std::rethrow_exception(impl_->failure); }
|
||||
if (impl_->stop_requested) { throw std::runtime_error("tty writer is already shut down"); }
|
||||
if (packet.size() > impl_->capacity_bytes) {
|
||||
throw std::runtime_error("RPH sweep exceeds tty queue capacity");
|
||||
}
|
||||
while (impl_->queued_bytes + packet.size() > impl_->capacity_bytes) {
|
||||
impl_->stats.frames_dropped += (impl_->packets.front().size() - 48U) / 8U;
|
||||
impl_->queued_bytes -= impl_->packets.front().size();
|
||||
impl_->packets.pop_front();
|
||||
++impl_->stats.ring_overflows;
|
||||
}
|
||||
impl_->queued_bytes += packet.size();
|
||||
impl_->packets.push_back(std::move(packet));
|
||||
impl_->data_ready_cv.notify_one();
|
||||
}
|
||||
|
||||
TtyProtocolWriter::StatsSnapshot TtyProtocolWriter::stats() const {
|
||||
if (!impl_) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
return impl_->stats;
|
||||
}
|
||||
|
||||
const std::string& TtyProtocolWriter::path() const {
|
||||
return path_;
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::throw_if_failed() const {
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::exception_ptr failure;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
failure = impl_->failure;
|
||||
}
|
||||
|
||||
if (failure) {
|
||||
std::rethrow_exception(failure);
|
||||
}
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::shutdown() {
|
||||
if (!impl_) {
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
impl_->stop_requested = true;
|
||||
}
|
||||
|
||||
impl_->data_ready_cv.notify_all();
|
||||
|
||||
if (impl_->worker.joinable()) {
|
||||
impl_->worker.join();
|
||||
}
|
||||
close_fd_if_open(impl_->fd);
|
||||
}
|
||||
|
||||
void TtyProtocolWriter::worker_loop() {
|
||||
for (;;) {
|
||||
std::vector<std::uint8_t> packet;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(impl_->mutex);
|
||||
impl_->data_ready_cv.wait(lock, [this]() {
|
||||
return impl_->stop_requested || impl_->failure || !impl_->packets.empty();
|
||||
});
|
||||
if (impl_->failure || impl_->stop_requested) { return; }
|
||||
packet = std::move(impl_->packets.front());
|
||||
impl_->packets.pop_front();
|
||||
impl_->queued_bytes -= packet.size();
|
||||
}
|
||||
// Once a packet starts writing it is indivisible, even if the reader
|
||||
// stalls. Overflow drops only complete packets still in the queue.
|
||||
const std::uint8_t* bytes = packet.data();
|
||||
std::size_t remaining = packet.size();
|
||||
while (remaining != 0U) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (impl_->stop_requested) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
const ssize_t written = ::write(impl_->fd, bytes, remaining);
|
||||
if (written < 0) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
if ((errno == EAGAIN) || (errno == EWOULDBLOCK) || (errno == EIO)) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (impl_->stop_requested) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
continue;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (!impl_->stop_requested) {
|
||||
impl_->failure = std::make_exception_ptr(
|
||||
std::runtime_error(io_error("Cannot write tty frame to", path_)));
|
||||
}
|
||||
impl_->data_ready_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
if (written == 0) {
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (!impl_->stop_requested) {
|
||||
impl_->failure = std::make_exception_ptr(
|
||||
std::runtime_error("tty write returned 0 bytes for '" + path_ + "'"));
|
||||
}
|
||||
impl_->data_ready_cv.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
bytes += static_cast<std::size_t>(written);
|
||||
remaining -= static_cast<std::size_t>(written);
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
impl_->stats.frames_written += (packet.size() - 48U) / 8U;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,52 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
/**
|
||||
* @brief One forward sweep acquired from the radar.
|
||||
*
|
||||
* All vectors must have equal size and aligned indices.
|
||||
*/
|
||||
struct SweepTrace {
|
||||
std::vector<float> frequency_hz{};
|
||||
std::vector<ipc::Complex32> s11{};
|
||||
std::vector<ipc::Complex32> s21{};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Minimal radar interface used by the sweep orchestrator.
|
||||
*
|
||||
* Implementations are expected to be lightweight: configuration is handled by
|
||||
* the Python layer, while this interface only opens/closes and acquires one
|
||||
* forward sweep.
|
||||
*/
|
||||
class RadarDriver {
|
||||
public:
|
||||
virtual ~RadarDriver() = default;
|
||||
|
||||
/** @brief Open underlying transport and prepare acquisition. */
|
||||
virtual void open() = 0;
|
||||
/** @brief Release all allocated resources. */
|
||||
virtual void close() = 0;
|
||||
/** @brief Acquire one sweep containing the forward traces exposed by the driver. */
|
||||
[[nodiscard]] virtual auto acquire_sweep() -> SweepTrace = 0;
|
||||
|
||||
/**
|
||||
* @brief Announce which switch combo the next `acquire_sweep` belongs to.
|
||||
*
|
||||
* Real radars are agnostic to this because the switch state itself decides
|
||||
* what they see. Mock drivers use it to synthesise per-combo variation so
|
||||
* downstream plots show eight distinct traces for an eight-combo run
|
||||
* instead of eight identical curves stacked on top of each other.
|
||||
*
|
||||
* Default implementation is a no-op so production drivers do not need to
|
||||
* override.
|
||||
*/
|
||||
virtual void set_active_combo(const ipc::ComboKey& /*combo*/) {}
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
-24
@@ -1,24 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
/**
|
||||
* @brief Unified RF switch interface consumed by the orchestrator.
|
||||
*/
|
||||
class SwitchDriver {
|
||||
public:
|
||||
virtual ~SwitchDriver() = default;
|
||||
|
||||
/** @brief Open underlying transport and set initial switch state. */
|
||||
virtual void open() = 0;
|
||||
/** @brief Release driver resources. */
|
||||
virtual void close() = 0;
|
||||
/** @brief Number of selectable positions exposed by this switch. */
|
||||
[[nodiscard]] virtual auto position_count() const -> std::uint32_t = 0;
|
||||
/** @brief Switch to a 0-based position. */
|
||||
virtual void switch_to(std::uint32_t position) = 0;
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
-414
@@ -1,414 +0,0 @@
|
||||
#include "../compact_m_k209_driver.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cctype>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace {
|
||||
|
||||
constexpr std::uint32_t kFloatBytes = 4U;
|
||||
constexpr std::uint32_t kComplexScalarCount = 2U;
|
||||
constexpr std::uint64_t kMaxBinaryBlockBytes = 512ULL * 1024ULL * 1024ULL;
|
||||
|
||||
[[nodiscard]] auto visa_resource(const std::string& resource) -> ViRsrc {
|
||||
return reinterpret_cast<ViRsrc>(const_cast<char*>(resource.c_str()));
|
||||
}
|
||||
|
||||
[[nodiscard]] auto visa_buffer(std::uint8_t* data) -> ViBuf {
|
||||
return reinterpret_cast<ViBuf>(data);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto visa_const_buffer(const char* data) -> ViBuf {
|
||||
return reinterpret_cast<ViBuf>(const_cast<char*>(data));
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_line_ending(std::uint8_t value) -> bool {
|
||||
return value == static_cast<std::uint8_t>('\n') || value == static_cast<std::uint8_t>('\r');
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_response_separator(std::uint8_t value) -> bool {
|
||||
return is_line_ending(value) || value == static_cast<std::uint8_t>(';');
|
||||
}
|
||||
|
||||
[[nodiscard]] auto checked_vi_count(std::uint64_t value) -> ViUInt32 {
|
||||
const auto max_count = static_cast<std::uint64_t>(std::numeric_limits<ViUInt32>::max());
|
||||
return static_cast<ViUInt32>(std::min(value, max_count));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
CompactMK209Driver::CompactMK209Driver(CompactMK209DriverSettings settings) : settings_(std::move(settings)) {}
|
||||
|
||||
CompactMK209Driver::~CompactMK209Driver() {
|
||||
try {
|
||||
close();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void CompactMK209Driver::open() {
|
||||
if (is_open_) {
|
||||
return;
|
||||
}
|
||||
if (settings_.resource.empty()) {
|
||||
throw std::runtime_error("K209 VISA resource must not be empty");
|
||||
}
|
||||
if (settings_.sweep.points < 2U) {
|
||||
throw std::runtime_error("K209 sweep points must be >= 2");
|
||||
}
|
||||
if (settings_.sweep.stop_hz < settings_.sweep.start_hz) {
|
||||
throw std::runtime_error("K209 sweep stop_hz must be >= start_hz");
|
||||
}
|
||||
if (!(settings_.sweep.if_bandwidth_hz > 0.0F)) {
|
||||
throw std::runtime_error("K209 IF bandwidth must be > 0");
|
||||
}
|
||||
|
||||
ViStatus status = viOpenDefaultRM(&resource_manager_);
|
||||
if (status < VI_SUCCESS) {
|
||||
throw std::runtime_error("Failed to open VISA resource manager: status=" + std::to_string(status));
|
||||
}
|
||||
|
||||
try {
|
||||
check_status(
|
||||
viOpen(resource_manager_, visa_resource(settings_.resource), VI_NULL, VI_NULL, &instrument_),
|
||||
"open K209 VISA resource"
|
||||
);
|
||||
check_status(
|
||||
viSetAttribute(instrument_, VI_ATTR_TMO_VALUE, static_cast<ViAttrState>(settings_.timeout_ms)),
|
||||
"set K209 VISA timeout"
|
||||
);
|
||||
check_status(
|
||||
viSetAttribute(instrument_, VI_ATTR_TERMCHAR_EN, static_cast<ViAttrState>(VI_FALSE)),
|
||||
"disable K209 VISA termchar"
|
||||
);
|
||||
write_command("*CLS");
|
||||
configure_device();
|
||||
} catch (...) {
|
||||
close();
|
||||
throw;
|
||||
}
|
||||
|
||||
is_open_ = true;
|
||||
}
|
||||
|
||||
void CompactMK209Driver::close() {
|
||||
if (instrument_ != VI_NULL) {
|
||||
viClose(instrument_);
|
||||
instrument_ = VI_NULL;
|
||||
}
|
||||
if (resource_manager_ != VI_NULL) {
|
||||
viClose(resource_manager_);
|
||||
resource_manager_ = VI_NULL;
|
||||
}
|
||||
is_open_ = false;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::acquire_sweep() -> SweepTrace {
|
||||
require_open();
|
||||
|
||||
auto traces = query_sweep_trace_pair(settings_.sweep.points);
|
||||
|
||||
SweepTrace trace{};
|
||||
trace.frequency_hz = frequency_hz_;
|
||||
trace.s11 = complex_trace_from_interleaved(traces.first);
|
||||
trace.s21 = complex_trace_from_interleaved(traces.second);
|
||||
return trace;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::query_identity() -> std::string {
|
||||
require_open();
|
||||
return trim_ascii(query_string("*IDN?"));
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::query_system_error() -> std::string {
|
||||
require_open();
|
||||
return trim_ascii(query_string("SYST:ERR?"));
|
||||
}
|
||||
|
||||
void CompactMK209Driver::configure_device() {
|
||||
if (settings_.preset_on_open) {
|
||||
write_command("SYST:PRES");
|
||||
expect_operation_complete("*OPC?", "wait for K209 preset");
|
||||
}
|
||||
|
||||
write_command("SENS:FREQ:STAR " + format_frequency(settings_.sweep.start_hz));
|
||||
write_command("SENS:FREQ:STOP " + format_frequency(settings_.sweep.stop_hz));
|
||||
write_command("SENS:SWE:POIN " + std::to_string(settings_.sweep.points));
|
||||
write_command("SENS:SWE:POIN:TIME 0");
|
||||
write_command("SENS:BAND " + format_frequency(settings_.sweep.if_bandwidth_hz));
|
||||
write_command("SOUR:POW " + format_power(settings_.sweep.power_dbm));
|
||||
write_command("SENS:AVER OFF");
|
||||
write_command("CALC:PAR1:DEF S21");
|
||||
write_command("CALC:PAR1:SEL");
|
||||
write_command("FORM:DATA REAL32");
|
||||
write_command("FORM:BORD SWAP");
|
||||
write_command("INIT:CONT ON");
|
||||
write_command("TRIG:SOUR BUS");
|
||||
expect_operation_complete("*OPC?", "wait for K209 setup");
|
||||
|
||||
frequency_hz_ = query_float_array("SENS:FREQ:DATA?", settings_.sweep.points);
|
||||
}
|
||||
|
||||
void CompactMK209Driver::expect_operation_complete(const std::string& command, const std::string& context) {
|
||||
const auto response = trim_ascii(query_string(command));
|
||||
if (response != "1") {
|
||||
throw std::runtime_error(context + " returned unexpected *OPC? response: " + response);
|
||||
}
|
||||
}
|
||||
|
||||
void CompactMK209Driver::write_command(const std::string& command) {
|
||||
require_open();
|
||||
|
||||
const std::string message = command + "\n";
|
||||
ViUInt32 written = 0;
|
||||
check_status(
|
||||
viWrite(instrument_, visa_const_buffer(message.data()), static_cast<ViUInt32>(message.size()), &written),
|
||||
"write K209 command: " + command
|
||||
);
|
||||
if (written != message.size()) {
|
||||
throw std::runtime_error("Incomplete K209 command write: " + command);
|
||||
}
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::query_string(const std::string& command) -> std::string {
|
||||
write_command(command);
|
||||
return read_line();
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::query_float_array(
|
||||
const std::string& command,
|
||||
std::uint32_t expected_values
|
||||
) -> std::vector<float> {
|
||||
write_command(command);
|
||||
return read_float_array_response(command, expected_values);
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::query_sweep_trace_pair(std::uint32_t points)
|
||||
-> std::pair<std::vector<float>, std::vector<float>> {
|
||||
write_command("TRIG:SING;*OPC?;:SENS:DATA:CORR? S11;:SENS:DATA:CORR? S21");
|
||||
const auto opc_response = read_ascii_token();
|
||||
if (opc_response != "1") {
|
||||
throw std::runtime_error("K209 sweep returned unexpected *OPC? response: " + opc_response);
|
||||
}
|
||||
|
||||
return {
|
||||
read_float_array_response("SENS:DATA:CORR? S11", points * kComplexScalarCount),
|
||||
read_float_array_response("SENS:DATA:CORR? S21", points * kComplexScalarCount),
|
||||
};
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_float_array_response(
|
||||
const std::string& context,
|
||||
std::uint32_t expected_values
|
||||
) -> std::vector<float> {
|
||||
auto values = float_array_from_little_endian(read_ieee_block());
|
||||
if (values.size() != expected_values) {
|
||||
throw std::runtime_error(
|
||||
"K209 response for " + context + " returned " + std::to_string(values.size()) +
|
||||
" float32 values, expected " + std::to_string(expected_values)
|
||||
);
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_ascii_token() -> std::string {
|
||||
require_open();
|
||||
|
||||
std::string token{};
|
||||
token.reserve(32);
|
||||
|
||||
while (true) {
|
||||
const auto byte = read_byte();
|
||||
if (is_response_separator(byte)) {
|
||||
if (!token.empty()) {
|
||||
return token;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
token.push_back(static_cast<char>(byte));
|
||||
if (token.size() > 64U * 1024U) {
|
||||
throw std::runtime_error("K209 ASCII response token is too long");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_line() -> std::string {
|
||||
require_open();
|
||||
|
||||
std::string line{};
|
||||
line.reserve(128);
|
||||
bool has_content = false;
|
||||
|
||||
while (true) {
|
||||
const auto byte = read_byte();
|
||||
if (!has_content && is_line_ending(byte)) {
|
||||
continue;
|
||||
}
|
||||
if (byte == static_cast<std::uint8_t>('\n')) {
|
||||
break;
|
||||
}
|
||||
if (byte != static_cast<std::uint8_t>('\r')) {
|
||||
line.push_back(static_cast<char>(byte));
|
||||
has_content = true;
|
||||
}
|
||||
if (line.size() > 64U * 1024U) {
|
||||
throw std::runtime_error("K209 ASCII response line is too long");
|
||||
}
|
||||
}
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_byte() -> std::uint8_t {
|
||||
auto bytes = read_exact(1);
|
||||
return bytes[0];
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_exact(std::uint64_t size) -> std::vector<std::uint8_t> {
|
||||
require_open();
|
||||
|
||||
std::vector<std::uint8_t> bytes(size);
|
||||
std::uint64_t offset = 0;
|
||||
while (offset < size) {
|
||||
ViUInt32 transferred = 0;
|
||||
const auto count = checked_vi_count(size - offset);
|
||||
check_status(
|
||||
viRead(instrument_, visa_buffer(bytes.data() + offset), count, &transferred),
|
||||
"read K209 response bytes"
|
||||
);
|
||||
if (transferred == 0U) {
|
||||
throw std::runtime_error("K209 VISA read returned zero bytes before expected payload was complete");
|
||||
}
|
||||
offset += transferred;
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::read_ieee_block() -> std::vector<std::uint8_t> {
|
||||
std::uint8_t marker = read_byte();
|
||||
while (is_response_separator(marker)) {
|
||||
marker = read_byte();
|
||||
}
|
||||
if (marker != static_cast<std::uint8_t>('#')) {
|
||||
throw std::runtime_error("K209 binary response does not start with IEEE block marker '#'");
|
||||
}
|
||||
|
||||
const auto digit = read_byte();
|
||||
if (digit != static_cast<std::uint8_t>('8')) {
|
||||
throw std::runtime_error("K209 binary response uses unsupported IEEE block header width");
|
||||
}
|
||||
|
||||
const auto size_text_bytes = read_exact(8);
|
||||
std::uint64_t payload_size = 0;
|
||||
for (const auto byte : size_text_bytes) {
|
||||
if (!std::isdigit(static_cast<unsigned char>(byte))) {
|
||||
throw std::runtime_error("K209 binary response has non-numeric payload size");
|
||||
}
|
||||
payload_size = (payload_size * 10ULL) + static_cast<std::uint64_t>(byte - static_cast<std::uint8_t>('0'));
|
||||
}
|
||||
if (payload_size == 0U || payload_size > kMaxBinaryBlockBytes) {
|
||||
throw std::runtime_error("K209 binary response payload size is out of allowed range");
|
||||
}
|
||||
if ((payload_size % kFloatBytes) != 0U) {
|
||||
throw std::runtime_error("K209 binary response payload size is not aligned to float32 values");
|
||||
}
|
||||
|
||||
return read_exact(payload_size);
|
||||
}
|
||||
|
||||
void CompactMK209Driver::check_status(ViStatus status, const std::string& context) const {
|
||||
if (status >= VI_SUCCESS) {
|
||||
return;
|
||||
}
|
||||
throw std::runtime_error(context + ": " + status_message(status));
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::status_message(ViStatus status) const -> std::string {
|
||||
std::array<ViChar, 256> description{};
|
||||
const auto session = instrument_ != VI_NULL ? instrument_ : resource_manager_;
|
||||
if (session != VI_NULL && viStatusDesc(session, status, description.data()) >= VI_SUCCESS) {
|
||||
return std::string(description.data()) + " (status=" + std::to_string(status) + ")";
|
||||
}
|
||||
return "VISA status=" + std::to_string(status);
|
||||
}
|
||||
|
||||
void CompactMK209Driver::require_open() const {
|
||||
if (instrument_ == VI_NULL) {
|
||||
throw std::runtime_error("K209 VISA instrument is not open");
|
||||
}
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::float_array_from_little_endian(std::vector<std::uint8_t> bytes) -> std::vector<float> {
|
||||
if ((bytes.size() % kFloatBytes) != 0U) {
|
||||
throw std::runtime_error("K209 binary float payload is not aligned to 4 bytes");
|
||||
}
|
||||
|
||||
std::vector<float> values(bytes.size() / kFloatBytes, 0.0F);
|
||||
for (std::size_t index = 0; index < values.size(); ++index) {
|
||||
const auto offset = index * kFloatBytes;
|
||||
const std::uint32_t bits = static_cast<std::uint32_t>(bytes[offset]) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 1U]) << 8U) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 2U]) << 16U) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 3U]) << 24U);
|
||||
std::memcpy(&values[index], &bits, sizeof(float));
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::complex_trace_from_interleaved(const std::vector<float>& values)
|
||||
-> std::vector<ipc::Complex32> {
|
||||
if ((values.size() % kComplexScalarCount) != 0U) {
|
||||
throw std::runtime_error("K209 complex trace payload has odd scalar count");
|
||||
}
|
||||
|
||||
std::vector<ipc::Complex32> trace{};
|
||||
trace.reserve(values.size() / kComplexScalarCount);
|
||||
for (std::size_t index = 0; index < values.size(); index += kComplexScalarCount) {
|
||||
trace.push_back(ipc::Complex32{.re = values[index], .im = values[index + 1U]});
|
||||
}
|
||||
return trace;
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::format_frequency(float value_hz) -> std::string {
|
||||
if (!std::isfinite(value_hz)) {
|
||||
throw std::runtime_error("K209 frequency value is not finite");
|
||||
}
|
||||
|
||||
std::ostringstream stream;
|
||||
stream.precision(9);
|
||||
stream << std::fixed << value_hz;
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::format_power(float value_dbm) -> std::string {
|
||||
if (!std::isfinite(value_dbm)) {
|
||||
throw std::runtime_error("K209 power value is not finite");
|
||||
}
|
||||
|
||||
std::ostringstream stream;
|
||||
stream.precision(3);
|
||||
stream << std::fixed << value_dbm;
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
auto CompactMK209Driver::trim_ascii(std::string value) -> std::string {
|
||||
const auto first = std::find_if_not(value.begin(), value.end(), [](unsigned char ch) {
|
||||
return std::isspace(ch) != 0;
|
||||
});
|
||||
const auto last = std::find_if_not(value.rbegin(), value.rend(), [](unsigned char ch) {
|
||||
return std::isspace(ch) != 0;
|
||||
}).base();
|
||||
if (first >= last) {
|
||||
return {};
|
||||
}
|
||||
return std::string(first, last);
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-71
@@ -1,71 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <visa.h>
|
||||
|
||||
#include "radar_driver.hpp"
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
struct CompactMK209DriverSettings {
|
||||
std::string resource{};
|
||||
config::RadarSweepSettings sweep{};
|
||||
std::uint32_t timeout_ms = 20'000;
|
||||
bool preset_on_open = true;
|
||||
};
|
||||
|
||||
class CompactMK209Driver final : public RadarDriver {
|
||||
public:
|
||||
explicit CompactMK209Driver(CompactMK209DriverSettings settings);
|
||||
~CompactMK209Driver() override;
|
||||
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto acquire_sweep() -> SweepTrace override;
|
||||
|
||||
[[nodiscard]] auto query_identity() -> std::string;
|
||||
[[nodiscard]] auto query_system_error() -> std::string;
|
||||
|
||||
private:
|
||||
void configure_device();
|
||||
void expect_operation_complete(const std::string& command, const std::string& context);
|
||||
|
||||
void write_command(const std::string& command);
|
||||
[[nodiscard]] auto query_string(const std::string& command) -> std::string;
|
||||
[[nodiscard]] auto query_float_array(const std::string& command, std::uint32_t expected_values)
|
||||
-> std::vector<float>;
|
||||
[[nodiscard]] auto query_sweep_trace_pair(std::uint32_t points)
|
||||
-> std::pair<std::vector<float>, std::vector<float>>;
|
||||
[[nodiscard]] auto read_float_array_response(const std::string& context, std::uint32_t expected_values)
|
||||
-> std::vector<float>;
|
||||
|
||||
[[nodiscard]] auto read_ascii_token() -> std::string;
|
||||
[[nodiscard]] auto read_line() -> std::string;
|
||||
[[nodiscard]] auto read_byte() -> std::uint8_t;
|
||||
[[nodiscard]] auto read_exact(std::uint64_t size) -> std::vector<std::uint8_t>;
|
||||
[[nodiscard]] auto read_ieee_block() -> std::vector<std::uint8_t>;
|
||||
|
||||
void check_status(ViStatus status, const std::string& context) const;
|
||||
[[nodiscard]] auto status_message(ViStatus status) const -> std::string;
|
||||
void require_open() const;
|
||||
|
||||
[[nodiscard]] static auto float_array_from_little_endian(std::vector<std::uint8_t> bytes) -> std::vector<float>;
|
||||
[[nodiscard]] static auto complex_trace_from_interleaved(const std::vector<float>& values)
|
||||
-> std::vector<ipc::Complex32>;
|
||||
[[nodiscard]] static auto format_frequency(float value_hz) -> std::string;
|
||||
[[nodiscard]] static auto format_power(float value_dbm) -> std::string;
|
||||
[[nodiscard]] static auto trim_ascii(std::string value) -> std::string;
|
||||
|
||||
CompactMK209DriverSettings settings_{};
|
||||
std::vector<float> frequency_hz_{};
|
||||
bool is_open_ = false;
|
||||
ViSession resource_manager_ = VI_NULL;
|
||||
ViSession instrument_ = VI_NULL;
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
-409
@@ -1,409 +0,0 @@
|
||||
#include "../librevna_minimal_driver.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "librevna_protocol_common.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace detail = radar::drivers::librevna::detail;
|
||||
namespace {
|
||||
|
||||
constexpr std::uint32_t kNativeAcquireMaxAttempts = 3U;
|
||||
|
||||
// Derive the budget the whole multi-point sweep is allowed to take before the
|
||||
// first datapoint must arrive (#7): a fixed setup cost plus the expected dwell
|
||||
// (points / IFBW) with a generous margin, clamped to the overall hard cap. The
|
||||
// per-gap stall timeout then governs progress once datapoints start flowing.
|
||||
[[nodiscard]] auto native_initial_sweep_budget(const config::RadarSweepSettings& sweep)
|
||||
-> std::chrono::milliseconds {
|
||||
const float points = std::max(1.0F, static_cast<float>(sweep.points));
|
||||
const float if_bw = std::max(1.0F, sweep.if_bandwidth_hz);
|
||||
const double dwell_ms =
|
||||
(static_cast<double>(points) / static_cast<double>(if_bw)) * 1000.0 *
|
||||
static_cast<double>(detail::kNativeSweepDwellMargin);
|
||||
const double total_ms = static_cast<double>(detail::kNativeSweepSetupBudgetMs) + dwell_ms;
|
||||
const auto budget = std::chrono::milliseconds(static_cast<std::int64_t>(total_ms));
|
||||
return std::clamp<std::chrono::milliseconds>(
|
||||
budget,
|
||||
std::chrono::milliseconds(detail::kNativeSweepPerGapTimeoutMs),
|
||||
std::chrono::milliseconds(detail::kNativeSweepHardCapMs)
|
||||
);
|
||||
}
|
||||
|
||||
// One synthetic GPR reflector. `range_m` is its physical depth, `reflection`
|
||||
// is the dimensionless complex reflection coefficient (|Γ| ≤ 1).
|
||||
struct MockTarget {
|
||||
float range_m;
|
||||
float reflection_magnitude;
|
||||
};
|
||||
|
||||
// Three reflectors at GPR-typical depths: a strong near-surface scatterer,
|
||||
// a mid-depth target, and a weak deeper one. The magnitudes are tuned so the
|
||||
// summed S21 stays within unit modulus across the sweep band.
|
||||
constexpr std::array<MockTarget, 3> kMockTargets = {{
|
||||
{0.45F, 0.55F},
|
||||
{1.30F, 0.30F},
|
||||
{2.90F, 0.18F},
|
||||
}};
|
||||
|
||||
// Group velocity in moderately wet soil (≈ c / sqrt(εr), εr ≈ 4). The choice
|
||||
// is what maps reflector depth to round-trip phase delay; it is held constant
|
||||
// to keep the simulator deterministic.
|
||||
constexpr float kGroundVelocityMps = 1.5e8F;
|
||||
|
||||
// Soil attenuation grows with frequency. Calibrated so a target at 3 m sees
|
||||
// roughly −20 dB extra loss at 6 GHz on top of geometric spreading.
|
||||
constexpr float kAttenuationCoeffPerMeterAtRefHz = 0.22F;
|
||||
constexpr float kAttenuationReferenceHz = 6e9F;
|
||||
|
||||
// Antenna mismatch dominates S11: simulate one shallow reflection right at
|
||||
// the connector, plus a small amount of cross-coupling from S21 targets.
|
||||
constexpr float kS11ConnectorReflection = 0.55F;
|
||||
constexpr float kS11ConnectorRangeM = 0.02F;
|
||||
constexpr float kS11CrossCouplingFactor = 0.06F;
|
||||
|
||||
// Noise floor in linear voltage units. Real LibreVNA hits ~ −90 dB at 1 kHz
|
||||
// IFBW; pick something a touch noisier so the GPR processor has to work.
|
||||
constexpr float kNoiseAmplitudeLinear = 0.004F;
|
||||
|
||||
// Minimum simulated dwell so 0-point or pathological configs don't busy-loop.
|
||||
constexpr auto kMockMinimumSweepDuration = std::chrono::microseconds(50);
|
||||
|
||||
// Compute the dwell time the mock pretends to spend on the device. Mirrors
|
||||
// the real LibreVNA contract: per-point dwell ≈ 1 / IFBW. Capped so absurd
|
||||
// configs (IFBW ≈ 0 or huge sweeps) cannot freeze the producer for hours.
|
||||
[[nodiscard]] auto mock_target_sweep_duration(const config::RadarSweepSettings& sweep)
|
||||
-> std::chrono::nanoseconds {
|
||||
const float points = std::max(1.0F, static_cast<float>(sweep.points));
|
||||
const float if_bw = std::max(1.0F, sweep.if_bandwidth_hz);
|
||||
const double seconds = static_cast<double>(points) / static_cast<double>(if_bw);
|
||||
const auto duration_ns = std::chrono::nanoseconds(
|
||||
static_cast<std::chrono::nanoseconds::rep>(seconds * 1e9)
|
||||
);
|
||||
constexpr auto kHardCap = std::chrono::seconds(5);
|
||||
return std::clamp<std::chrono::nanoseconds>(duration_ns, kMockMinimumSweepDuration, kHardCap);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_retryable_native_acquire_error(std::string_view message) -> bool {
|
||||
// A stop request must not be retried: drain to the caller immediately (#26).
|
||||
if (message.find("aborted by stop request") != std::string_view::npos) {
|
||||
return false;
|
||||
}
|
||||
|
||||
constexpr std::array<std::string_view, 8> kRetryableSubstrings = {
|
||||
"Timeout waiting for expected LibreVNA packet type",
|
||||
"Timeout waiting for LibreVNA ACK",
|
||||
"LibreVNA returned NACK",
|
||||
"Failed to read LibreVNA USB bulk packet",
|
||||
"Failed to write LibreVNA USB bulk packet",
|
||||
// #31: a transient USB error often triggers a brief device re-enumeration,
|
||||
// so the device may be momentarily absent or its handle invalidated when
|
||||
// we reconnect. Treat these as retryable; the reconnect path absorbs the
|
||||
// re-enumeration with a short bounded discovery backoff.
|
||||
"No compatible LibreVNA USB device found",
|
||||
"LibreVNA native handle is not open",
|
||||
"LibreVNA USB handle is not open",
|
||||
};
|
||||
|
||||
return std::any_of(
|
||||
kRetryableSubstrings.begin(),
|
||||
kRetryableSubstrings.end(),
|
||||
[message](std::string_view needle) {
|
||||
return message.find(needle) != std::string_view::npos;
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
LibreVnaMinimalDriver::LibreVnaMinimalDriver(LibreVnaMinimalDriverSettings settings) : settings_(std::move(settings)) {}
|
||||
|
||||
void LibreVnaMinimalDriver::open() {
|
||||
if (is_open_) {
|
||||
return;
|
||||
}
|
||||
if (settings_.sweep.points == 0U) {
|
||||
throw std::runtime_error("Radar sweep points must be > 0");
|
||||
}
|
||||
|
||||
if (settings_.mode == config::DriverMode::Native) {
|
||||
open_native();
|
||||
}
|
||||
|
||||
is_open_ = true;
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::close() {
|
||||
if (!is_open_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (settings_.mode == config::DriverMode::Native) {
|
||||
close_native();
|
||||
}
|
||||
|
||||
is_open_ = false;
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::set_active_combo(const ipc::ComboKey& combo) {
|
||||
active_combo_ = combo;
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::acquire_sweep() -> SweepTrace {
|
||||
if (!is_open_) {
|
||||
throw std::runtime_error("Radar driver is not open");
|
||||
}
|
||||
|
||||
++sweep_index_;
|
||||
switch (settings_.mode) {
|
||||
case config::DriverMode::Mock:
|
||||
return acquire_mock();
|
||||
case config::DriverMode::Native: {
|
||||
std::exception_ptr last_exception{};
|
||||
std::string last_message{};
|
||||
|
||||
for (std::uint32_t attempt = 1; attempt <= kNativeAcquireMaxAttempts; ++attempt) {
|
||||
try {
|
||||
return acquire_native();
|
||||
} catch (const std::exception& exception) {
|
||||
last_exception = std::current_exception();
|
||||
last_message = exception.what();
|
||||
|
||||
if (!is_retryable_native_acquire_error(last_message) || attempt == kNativeAcquireMaxAttempts) {
|
||||
break;
|
||||
}
|
||||
if (detail::native_stop_requested()) {
|
||||
break; // #26: do not waste reconnect attempts while shutting down.
|
||||
}
|
||||
|
||||
// #31: recover from transient USB/protocol stalls without
|
||||
// destroying the libusb_context. Release only the interface
|
||||
// and handle, then reopen against the surviving context so
|
||||
// we ride out a brief device re-enumeration instead of
|
||||
// re-initialising libusb from scratch on every retry.
|
||||
rx_buffer_.clear();
|
||||
packet_queue_.clear();
|
||||
if (usb_handle_ != nullptr && interface_claimed_) {
|
||||
libusb_release_interface(usb_handle_, detail::kUsbInterface);
|
||||
interface_claimed_ = false;
|
||||
}
|
||||
if (usb_handle_ != nullptr) {
|
||||
libusb_close(usb_handle_);
|
||||
usb_handle_ = nullptr;
|
||||
}
|
||||
protocol_version_ = 0;
|
||||
device_num_ports_ = 0;
|
||||
open_native(); // Reuses the surviving usb_context_ and retries discovery.
|
||||
}
|
||||
}
|
||||
|
||||
if (last_exception != nullptr) {
|
||||
std::rethrow_exception(last_exception);
|
||||
}
|
||||
throw std::runtime_error("Native acquisition failed without exception detail");
|
||||
}
|
||||
default:
|
||||
throw std::runtime_error("Unsupported radar driver mode");
|
||||
}
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
|
||||
// Synthesise a frequency-domain VNA response containing several discrete
|
||||
// reflectors at known depths, plus light Gaussian noise. The sweep dwells
|
||||
// for a realistic duration derived from the configured IF bandwidth so
|
||||
// downstream stages cannot be flooded faster than a real device would
|
||||
// produce data.
|
||||
const auto started_at = std::chrono::steady_clock::now();
|
||||
const auto target_duration = mock_target_sweep_duration(settings_.sweep);
|
||||
|
||||
SweepTrace trace{};
|
||||
trace.frequency_hz.reserve(settings_.sweep.points);
|
||||
trace.s11.reserve(settings_.sweep.points);
|
||||
trace.s21.reserve(settings_.sweep.points);
|
||||
|
||||
const float span_hz = settings_.sweep.stop_hz - settings_.sweep.start_hz;
|
||||
const float denominator =
|
||||
settings_.sweep.points > 1U ? static_cast<float>(settings_.sweep.points - 1U) : 1.0F;
|
||||
|
||||
// A tiny per-sweep range drift gives the rendered B-scan a visible motion
|
||||
// signature so the simulator does not look frozen.
|
||||
const float range_drift_m =
|
||||
0.01F * std::sin(0.07F * static_cast<float>(sweep_index_));
|
||||
|
||||
// Combo-dependent variation. Without this the mock returns near-identical
|
||||
// S21 for every (input, output) combo and an eight-combo pass-through plot
|
||||
// collapses into a single visible trace. The factors below are arbitrary
|
||||
// but chosen small enough that the overall response stays in a reasonable
|
||||
// band and large enough that each pair is visually distinct.
|
||||
const auto input_pos = static_cast<float>(active_combo_.input_pos);
|
||||
const auto output_pos = static_cast<float>(active_combo_.output_pos);
|
||||
const float combo_amplitude_gain = 0.55F + 0.08F * input_pos + 0.05F * output_pos;
|
||||
const float combo_phase_offset = 0.4F * input_pos + 0.9F * output_pos;
|
||||
const float combo_range_offset_m = 0.05F * input_pos + 0.12F * output_pos;
|
||||
|
||||
// Deterministic-per-sweep noise so two consecutive frames look distinct
|
||||
// but the test stays reproducible for any given sweep index.
|
||||
std::mt19937 noise_engine(
|
||||
static_cast<std::uint32_t>(0x9E3779B9ULL ^ sweep_index_)
|
||||
);
|
||||
std::normal_distribution<float> noise_dist(0.0F, kNoiseAmplitudeLinear);
|
||||
|
||||
for (std::uint32_t point = 0; point < settings_.sweep.points; ++point) {
|
||||
const float ratio = static_cast<float>(point) / denominator;
|
||||
const float frequency_hz = settings_.sweep.start_hz + span_hz * ratio;
|
||||
const float frequency_scale = frequency_hz / kAttenuationReferenceHz;
|
||||
|
||||
std::complex<float> s21_total{0.0F, 0.0F};
|
||||
std::complex<float> s11_total{0.0F, 0.0F};
|
||||
|
||||
for (const auto& target : kMockTargets) {
|
||||
const float range_m = target.range_m + range_drift_m + combo_range_offset_m;
|
||||
// Round-trip phase: 2π·f·(2R/v).
|
||||
const float round_trip_phase =
|
||||
2.0F * detail::kPi * frequency_hz * (2.0F * range_m / kGroundVelocityMps);
|
||||
// Geometric spreading: 1/(1 + R) keeps near-zero ranges finite.
|
||||
const float spreading = 1.0F / (1.0F + range_m);
|
||||
// Frequency-dependent soil attenuation in linear amplitude.
|
||||
const float attenuation =
|
||||
std::exp(-kAttenuationCoeffPerMeterAtRefHz * range_m * frequency_scale);
|
||||
|
||||
const std::complex<float> contribution = std::polar<float>(
|
||||
target.reflection_magnitude * spreading * attenuation * combo_amplitude_gain,
|
||||
-round_trip_phase + combo_phase_offset
|
||||
);
|
||||
s21_total += contribution;
|
||||
s11_total += kS11CrossCouplingFactor * contribution;
|
||||
}
|
||||
|
||||
// Antenna mismatch dominates the near-field S11 response.
|
||||
const float antenna_phase =
|
||||
2.0F * detail::kPi * frequency_hz * (2.0F * kS11ConnectorRangeM / kGroundVelocityMps);
|
||||
s11_total += std::polar<float>(kS11ConnectorReflection, -antenna_phase);
|
||||
|
||||
// Independent noise per channel; complex variance ≈ kNoiseAmplitudeLinear².
|
||||
s21_total += std::complex<float>(noise_dist(noise_engine), noise_dist(noise_engine));
|
||||
s11_total += std::complex<float>(noise_dist(noise_engine), noise_dist(noise_engine));
|
||||
|
||||
trace.frequency_hz.push_back(frequency_hz);
|
||||
trace.s11.push_back({.re = s11_total.real(), .im = s11_total.imag()});
|
||||
trace.s21.push_back({.re = s21_total.real(), .im = s21_total.imag()});
|
||||
}
|
||||
|
||||
// Honour the IFBW-derived dwell time. If generation already took longer
|
||||
// than the simulated device would have needed (huge `points` × CPU jitter)
|
||||
// we skip the sleep so the producer does not fall further behind.
|
||||
const auto elapsed = std::chrono::steady_clock::now() - started_at;
|
||||
if (elapsed < target_duration) {
|
||||
std::this_thread::sleep_for(target_duration - elapsed);
|
||||
}
|
||||
|
||||
return trace;
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::acquire_native() -> SweepTrace {
|
||||
if (usb_handle_ == nullptr) {
|
||||
throw std::runtime_error("LibreVNA native handle is not open");
|
||||
}
|
||||
|
||||
// Drop stale datapoints left in queue by previous operations.
|
||||
while (pop_packet(detail::kPacketVnaDatapoint).has_value()) {
|
||||
}
|
||||
|
||||
send_packet_no_payload(detail::kPacketInitiateSweep, true);
|
||||
|
||||
SweepTrace trace{};
|
||||
trace.frequency_hz.assign(settings_.sweep.points, 0.0F);
|
||||
trace.s11.assign(settings_.sweep.points, ipc::Complex32{});
|
||||
trace.s21.assign(settings_.sweep.points, ipc::Complex32{});
|
||||
|
||||
std::vector<std::uint8_t> received(settings_.sweep.points, 0U);
|
||||
std::uint32_t received_count = 0;
|
||||
|
||||
// #7: the per-datapoint deadline is rebased every time a fresh point lands
|
||||
// (or the first point must arrive within the size-derived budget), so a
|
||||
// large sweep is not killed by one fixed ~1.5s timeout. An independent hard
|
||||
// cap bounds the whole sweep against a device that streams forever.
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
const auto hard_cap_deadline = now + std::chrono::milliseconds(detail::kNativeSweepHardCapMs);
|
||||
auto gap_deadline = std::min(now + native_initial_sweep_budget(settings_.sweep), hard_cap_deadline);
|
||||
|
||||
while (received_count < settings_.sweep.points) {
|
||||
// #26: honour a stop request observed during the (chunked) USB wait so
|
||||
// SIGTERM aborts the sweep promptly instead of after the full budget.
|
||||
if (detail::native_stop_requested()) {
|
||||
throw std::runtime_error("Native sweep aborted by stop request");
|
||||
}
|
||||
if (std::chrono::steady_clock::now() >= hard_cap_deadline) {
|
||||
throw std::runtime_error(
|
||||
"Native sweep exceeded hard cap (" + std::to_string(received_count) + "/" +
|
||||
std::to_string(settings_.sweep.points) + " points received)"
|
||||
);
|
||||
}
|
||||
|
||||
const auto wait_deadline = std::min(gap_deadline, hard_cap_deadline);
|
||||
|
||||
NativePacket packet{};
|
||||
try {
|
||||
packet = wait_for_packet(detail::kPacketVnaDatapoint, wait_deadline);
|
||||
} catch (const std::exception& exception) {
|
||||
throw std::runtime_error(
|
||||
"Timeout while collecting VNADatapoints (" + std::to_string(received_count) + "/" +
|
||||
std::to_string(settings_.sweep.points) + " points received): " + exception.what()
|
||||
);
|
||||
}
|
||||
|
||||
DecodedVnaDatapoint datapoint{};
|
||||
if (!decode_vna_datapoint_traces(packet.payload, datapoint)) {
|
||||
throw std::runtime_error("Failed to decode traces from VNADatapoint packet");
|
||||
}
|
||||
if (datapoint.point_number >= settings_.sweep.points) {
|
||||
throw std::runtime_error("Received out-of-range VNADatapoint index");
|
||||
}
|
||||
|
||||
if (received[datapoint.point_number] == 0U) {
|
||||
received[datapoint.point_number] = 1U;
|
||||
++received_count;
|
||||
}
|
||||
|
||||
trace.frequency_hz[datapoint.point_number] = datapoint.frequency_hz;
|
||||
trace.s11[datapoint.point_number] = datapoint.s11;
|
||||
trace.s21[datapoint.point_number] = datapoint.s21;
|
||||
|
||||
// Extend the per-gap stall timeout now that progress was made.
|
||||
gap_deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::milliseconds(detail::kNativeSweepPerGapTimeoutMs);
|
||||
}
|
||||
|
||||
return trace;
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::validate_device_info_payload(std::span<const std::uint8_t> payload) const {
|
||||
if (payload.size() != detail::kDeviceInfoPayloadSize) {
|
||||
throw std::runtime_error("Unexpected DeviceInfo payload size");
|
||||
}
|
||||
|
||||
const auto protocol = detail::read_u16_le(payload, detail::kDeviceInfoProtocolOffset);
|
||||
if (protocol != detail::kProtocolV14) {
|
||||
throw std::runtime_error("Unsupported LibreVNA protocol version (expected v14)");
|
||||
}
|
||||
|
||||
const auto ports = payload[detail::kDeviceInfoNumPortsOffset];
|
||||
if (ports < 2U) {
|
||||
throw std::runtime_error("LibreVNA reported invalid number of ports");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-148
@@ -1,148 +0,0 @@
|
||||
#include "../librevna_minimal_driver.hpp"
|
||||
|
||||
#include <array>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
#include "librevna_protocol_common.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace detail = radar::drivers::librevna::detail;
|
||||
|
||||
auto LibreVnaMinimalDriver::encode_frame(
|
||||
std::uint8_t packet_type,
|
||||
std::span<const std::uint8_t> payload
|
||||
) -> std::vector<std::uint8_t> {
|
||||
const auto length = payload.size() + detail::kFrameOverheadBytes;
|
||||
if (length > 0xFFFFU) {
|
||||
throw std::runtime_error("Protocol frame is too large");
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> frame(length, 0);
|
||||
frame[0] = detail::kFrameHeader;
|
||||
detail::write_u16_le(frame, 1, static_cast<std::uint16_t>(length));
|
||||
frame[3] = packet_type;
|
||||
std::copy(payload.begin(), payload.end(), frame.begin() + 4);
|
||||
|
||||
std::uint32_t crc = 0;
|
||||
if (packet_type != detail::kPacketVnaDatapoint) {
|
||||
crc = crc32(std::span<const std::uint8_t>(frame.data(), frame.size() - 4U));
|
||||
}
|
||||
detail::write_u32_le(frame, frame.size() - 4U, crc);
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::decode_vna_datapoint_traces(
|
||||
std::span<const std::uint8_t> payload,
|
||||
DecodedVnaDatapoint& datapoint_out
|
||||
) -> bool {
|
||||
// VNADatapoint payload layout:
|
||||
// [0..7]=freq_or_time, [8..9]=cdbm, [10..11]=point_number,
|
||||
// followed by N tuples of {real:f32, imag:f32, flags:u8}.
|
||||
if (payload.size() < 12U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto values_block = payload.size() - 12U;
|
||||
if ((values_block % 9U) != 0U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto num_values = values_block / 9U;
|
||||
if (num_values == 0U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
datapoint_out = DecodedVnaDatapoint{};
|
||||
datapoint_out.point_number = detail::read_u16_le(payload, 10);
|
||||
datapoint_out.frequency_hz = static_cast<float>(detail::read_u64_le(payload, 0));
|
||||
|
||||
const auto real_offset = 12U;
|
||||
const auto imag_offset = real_offset + (4U * num_values);
|
||||
const auto flags_offset = imag_offset + (4U * num_values);
|
||||
|
||||
std::array<std::complex<float>, 8> ref_by_stage{};
|
||||
std::array<std::complex<float>, 8> s11_measured_by_stage{};
|
||||
std::array<std::complex<float>, 8> s21_measured_by_stage{};
|
||||
std::array<bool, 8> has_ref{};
|
||||
std::array<bool, 8> has_s11_measured{};
|
||||
std::array<bool, 8> has_s21_measured{};
|
||||
constexpr float kReferenceMagnitudeSquaredEpsilon = 1e-12F;
|
||||
|
||||
for (std::size_t index = 0; index < num_values; ++index) {
|
||||
const auto flags = payload[flags_offset + index];
|
||||
const auto stage = static_cast<std::size_t>(flags >> 5U);
|
||||
if (stage >= ref_by_stage.size()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto value = std::complex<float>(
|
||||
detail::read_f32_le(payload, real_offset + (4U * index)),
|
||||
detail::read_f32_le(payload, imag_offset + (4U * index))
|
||||
);
|
||||
|
||||
const bool is_reference = (flags & detail::kReferenceFlagMask) != 0U;
|
||||
if ((flags & detail::kPort1Mask) != 0U && is_reference) {
|
||||
ref_by_stage[stage] = value;
|
||||
has_ref[stage] = true;
|
||||
}
|
||||
if ((flags & detail::kPort1Mask) != 0U && !is_reference) {
|
||||
s11_measured_by_stage[stage] = value;
|
||||
has_s11_measured[stage] = true;
|
||||
}
|
||||
if ((flags & detail::kPort2Mask) != 0U && !is_reference) {
|
||||
s21_measured_by_stage[stage] = value;
|
||||
has_s21_measured[stage] = true;
|
||||
}
|
||||
}
|
||||
|
||||
// For the forward sweep used in this project, port1 reference is the
|
||||
// incident signal, port1 measured is reflection (S11 numerator), and
|
||||
// port2 measured is transmission (S21 numerator).
|
||||
for (std::size_t stage = 0; stage < ref_by_stage.size(); ++stage) {
|
||||
if (!has_ref[stage]) {
|
||||
continue;
|
||||
}
|
||||
if (std::norm(ref_by_stage[stage]) <= kReferenceMagnitudeSquaredEpsilon) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (has_s11_measured[stage]) {
|
||||
const auto s11_ratio = s11_measured_by_stage[stage] / ref_by_stage[stage];
|
||||
datapoint_out.s11.re = s11_ratio.real();
|
||||
datapoint_out.s11.im = s11_ratio.imag();
|
||||
datapoint_out.has_s11 = true;
|
||||
}
|
||||
if (has_s21_measured[stage]) {
|
||||
const auto s21_ratio = s21_measured_by_stage[stage] / ref_by_stage[stage];
|
||||
datapoint_out.s21.re = s21_ratio.real();
|
||||
datapoint_out.s21.im = s21_ratio.imag();
|
||||
datapoint_out.has_s21 = true;
|
||||
}
|
||||
if (datapoint_out.has_s21) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::crc32(std::span<const std::uint8_t> data) -> std::uint32_t {
|
||||
std::uint32_t crc = 0xFFFFFFFFU;
|
||||
for (const auto byte : data) {
|
||||
crc ^= static_cast<std::uint32_t>(byte);
|
||||
for (int bit = 0; bit < 8; ++bit) {
|
||||
const auto lsb = crc & 1U;
|
||||
crc >>= 1U;
|
||||
if (lsb != 0U) {
|
||||
crc ^= 0xEDB88320U;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ~crc;
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-394
@@ -1,394 +0,0 @@
|
||||
#include "../librevna_minimal_driver.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "librevna_protocol_common.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace detail = radar::drivers::librevna::detail;
|
||||
namespace {
|
||||
|
||||
[[nodiscard]] auto libusb_error_message(const std::string& prefix, int status_code) -> std::string {
|
||||
return prefix + ": " + libusb_error_name(status_code);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_supported_vid_pid(std::uint16_t vendor_id, std::uint16_t product_id) -> bool {
|
||||
return std::any_of(
|
||||
detail::kSupportedUsbIds.begin(),
|
||||
detail::kSupportedUsbIds.end(),
|
||||
[vendor_id, product_id](const auto& pair) {
|
||||
return pair.first == vendor_id && pair.second == product_id;
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_serial_string(libusb_device_handle* handle, std::uint8_t serial_index) -> std::string {
|
||||
if (handle == nullptr || serial_index == 0U) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::array<unsigned char, 256> serial_bytes{};
|
||||
const auto length = libusb_get_string_descriptor_ascii(
|
||||
handle,
|
||||
serial_index,
|
||||
serial_bytes.data(),
|
||||
static_cast<int>(serial_bytes.size())
|
||||
);
|
||||
if (length <= 0) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return std::string(
|
||||
reinterpret_cast<const char*>(serial_bytes.data()),
|
||||
reinterpret_cast<const char*>(serial_bytes.data() + length)
|
||||
);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto find_matching_device_handle(
|
||||
libusb_context* usb_context,
|
||||
const std::string& expected_serial
|
||||
) -> libusb_device_handle* {
|
||||
libusb_device** devices = nullptr;
|
||||
const auto device_count = libusb_get_device_list(usb_context, &devices);
|
||||
if (device_count < 0) {
|
||||
throw std::runtime_error(
|
||||
libusb_error_message("Failed to enumerate USB devices", static_cast<int>(device_count))
|
||||
);
|
||||
}
|
||||
|
||||
libusb_device_handle* selected_handle = nullptr;
|
||||
for (ssize_t index = 0; index < device_count; ++index) {
|
||||
auto* device = devices[index];
|
||||
if (device == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
libusb_device_descriptor descriptor{};
|
||||
const auto descriptor_status = libusb_get_device_descriptor(device, &descriptor);
|
||||
if (descriptor_status != LIBUSB_SUCCESS) {
|
||||
continue;
|
||||
}
|
||||
if (!is_supported_vid_pid(descriptor.idVendor, descriptor.idProduct)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
libusb_device_handle* candidate_handle = nullptr;
|
||||
if (libusb_open(device, &candidate_handle) != LIBUSB_SUCCESS || candidate_handle == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!expected_serial.empty()) {
|
||||
const auto serial = parse_serial_string(candidate_handle, descriptor.iSerialNumber);
|
||||
if (serial != expected_serial) {
|
||||
libusb_close(candidate_handle);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
selected_handle = candidate_handle;
|
||||
break;
|
||||
}
|
||||
|
||||
libusb_free_device_list(devices, 1);
|
||||
return selected_handle;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void LibreVnaMinimalDriver::open_native() {
|
||||
if (usb_handle_ != nullptr) {
|
||||
throw std::runtime_error("LibreVNA native state is already initialized");
|
||||
}
|
||||
|
||||
// #26: observe SIGINT/SIGTERM (chaining to main()'s handler) so the blocking
|
||||
// USB loops can bail out promptly during shutdown. Safe to call repeatedly.
|
||||
detail::install_native_stop_observer();
|
||||
|
||||
// #31: keep the libusb_context alive across reconnect retries. Only create a
|
||||
// fresh context the first time; reconnect paths reuse the surviving context
|
||||
// and merely reopen the handle, avoiding a full libusb teardown/re-init.
|
||||
if (usb_context_ == nullptr) {
|
||||
const auto init_status = libusb_init(&usb_context_);
|
||||
if (init_status != LIBUSB_SUCCESS) {
|
||||
usb_context_ = nullptr;
|
||||
throw std::runtime_error(libusb_error_message("Failed to initialize libusb", init_status));
|
||||
}
|
||||
}
|
||||
|
||||
try {
|
||||
// #31: retry discovery with a short bounded backoff so a brief device
|
||||
// re-enumeration (common right after a transient USB error) is absorbed
|
||||
// instead of surfacing as a hard "device not found".
|
||||
libusb_device_handle* selected_handle = nullptr;
|
||||
for (int attempt = 0; attempt < detail::kNativeDiscoveryRetryAttempts; ++attempt) {
|
||||
if (detail::native_stop_requested()) {
|
||||
break; // #26: abandon discovery promptly during shutdown.
|
||||
}
|
||||
selected_handle = find_matching_device_handle(usb_context_, settings_.serial);
|
||||
if (selected_handle != nullptr) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(detail::kNativeDiscoveryRetryDelayMs));
|
||||
}
|
||||
|
||||
if (selected_handle == nullptr) {
|
||||
const auto serial_hint = settings_.serial.empty() ? std::string() : " for serial '" + settings_.serial + "'";
|
||||
throw std::runtime_error("No compatible LibreVNA USB device found" + serial_hint);
|
||||
}
|
||||
|
||||
usb_handle_ = selected_handle;
|
||||
|
||||
const auto auto_detach_status = libusb_set_auto_detach_kernel_driver(usb_handle_, 1);
|
||||
if (auto_detach_status != LIBUSB_SUCCESS && auto_detach_status != LIBUSB_ERROR_NOT_SUPPORTED) {
|
||||
throw std::runtime_error(
|
||||
libusb_error_message("Failed to configure USB auto-detach kernel driver", auto_detach_status)
|
||||
);
|
||||
}
|
||||
|
||||
const auto claim_status = libusb_claim_interface(usb_handle_, detail::kUsbInterface);
|
||||
if (claim_status != LIBUSB_SUCCESS) {
|
||||
throw std::runtime_error(libusb_error_message("Failed to claim LibreVNA USB interface", claim_status));
|
||||
}
|
||||
interface_claimed_ = true;
|
||||
|
||||
rx_buffer_.clear();
|
||||
packet_queue_.clear();
|
||||
|
||||
send_packet_no_payload(detail::kPacketRequestDeviceInfo, true);
|
||||
const auto info_packet = wait_for_packet(
|
||||
detail::kPacketDeviceInfo,
|
||||
std::chrono::steady_clock::now() + std::chrono::seconds(detail::kDeviceInfoTimeoutSeconds)
|
||||
);
|
||||
validate_device_info_payload(info_packet.payload);
|
||||
protocol_version_ = detail::read_u16_le(info_packet.payload, detail::kDeviceInfoProtocolOffset);
|
||||
device_num_ports_ = info_packet.payload[detail::kDeviceInfoNumPortsOffset];
|
||||
} catch (...) {
|
||||
close_native();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::close_native() {
|
||||
rx_buffer_.clear();
|
||||
packet_queue_.clear();
|
||||
|
||||
if (usb_handle_ != nullptr && interface_claimed_) {
|
||||
libusb_release_interface(usb_handle_, detail::kUsbInterface);
|
||||
interface_claimed_ = false;
|
||||
}
|
||||
|
||||
if (usb_handle_ != nullptr) {
|
||||
libusb_close(usb_handle_);
|
||||
usb_handle_ = nullptr;
|
||||
}
|
||||
|
||||
if (usb_context_ != nullptr) {
|
||||
libusb_exit(usb_context_);
|
||||
usb_context_ = nullptr;
|
||||
}
|
||||
|
||||
protocol_version_ = 0;
|
||||
device_num_ports_ = 0;
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::send_packet(
|
||||
std::uint8_t packet_type,
|
||||
std::span<const std::uint8_t> payload,
|
||||
bool require_ack
|
||||
) {
|
||||
if (usb_handle_ == nullptr) {
|
||||
throw std::runtime_error("Cannot send packet: LibreVNA USB handle is not open");
|
||||
}
|
||||
|
||||
auto frame = encode_frame(packet_type, payload);
|
||||
|
||||
int transferred = 0;
|
||||
const auto transfer_status = libusb_bulk_transfer(
|
||||
usb_handle_,
|
||||
detail::kEndpointOut,
|
||||
reinterpret_cast<unsigned char*>(frame.data()),
|
||||
static_cast<int>(frame.size()),
|
||||
&transferred,
|
||||
detail::kUsbWriteTimeoutMs
|
||||
);
|
||||
if (transfer_status != LIBUSB_SUCCESS) {
|
||||
throw std::runtime_error(libusb_error_message("Failed to write LibreVNA USB bulk packet", transfer_status));
|
||||
}
|
||||
if (transferred != static_cast<int>(frame.size())) {
|
||||
throw std::runtime_error("Incomplete LibreVNA USB bulk write");
|
||||
}
|
||||
|
||||
if (require_ack) {
|
||||
wait_for_ack(std::chrono::steady_clock::now() + std::chrono::milliseconds(detail::kAckTimeoutMs));
|
||||
}
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::send_packet_no_payload(std::uint8_t packet_type, bool require_ack) {
|
||||
send_packet(packet_type, std::span<const std::uint8_t>{}, require_ack);
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::wait_for_ack(std::chrono::steady_clock::time_point deadline) {
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
// #26: bail out promptly on SIGTERM/SIGINT rather than blocking until the
|
||||
// ACK deadline expires.
|
||||
if (detail::native_stop_requested()) {
|
||||
throw std::runtime_error("LibreVNA ACK wait aborted by stop request");
|
||||
}
|
||||
|
||||
std::optional<NativePacket> ack_or_nack{};
|
||||
for (auto iter = packet_queue_.begin(); iter != packet_queue_.end(); ++iter) {
|
||||
if (iter->packet_type != detail::kPacketAck && iter->packet_type != detail::kPacketNack) {
|
||||
continue;
|
||||
}
|
||||
ack_or_nack = std::move(*iter);
|
||||
packet_queue_.erase(iter);
|
||||
break;
|
||||
}
|
||||
|
||||
if (ack_or_nack.has_value()) {
|
||||
if (ack_or_nack->packet_type == detail::kPacketAck) {
|
||||
return;
|
||||
}
|
||||
throw std::runtime_error("LibreVNA returned NACK");
|
||||
}
|
||||
|
||||
pump_usb(deadline);
|
||||
}
|
||||
|
||||
throw std::runtime_error("Timeout waiting for LibreVNA ACK");
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::wait_for_packet(
|
||||
std::uint8_t expected_type,
|
||||
std::chrono::steady_clock::time_point deadline
|
||||
) -> NativePacket {
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
if (auto packet = pop_packet(expected_type); packet.has_value()) {
|
||||
return *packet;
|
||||
}
|
||||
// #26: re-check the stop flag between short USB polls so a shutdown is
|
||||
// honoured within a few hundred ms even on a long sweep deadline.
|
||||
if (detail::native_stop_requested()) {
|
||||
throw std::runtime_error("LibreVNA packet wait aborted by stop request");
|
||||
}
|
||||
pump_usb(deadline);
|
||||
}
|
||||
|
||||
throw std::runtime_error("Timeout waiting for expected LibreVNA packet type");
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::pump_usb(std::chrono::steady_clock::time_point deadline) {
|
||||
if (usb_handle_ == nullptr) {
|
||||
throw std::runtime_error("Cannot read packets: LibreVNA USB handle is not open");
|
||||
}
|
||||
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (now >= deadline) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto remaining_ms = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - now).count();
|
||||
const auto timeout_ms = static_cast<unsigned int>(
|
||||
std::clamp<std::int64_t>(remaining_ms, detail::kUsbReadPollMinTimeoutMs, detail::kUsbReadPollMaxTimeoutMs)
|
||||
);
|
||||
|
||||
std::array<unsigned char, detail::kUsbReadChunkBytes> buffer{};
|
||||
int transferred = 0;
|
||||
const auto transfer_status = libusb_bulk_transfer(
|
||||
usb_handle_,
|
||||
detail::kEndpointIn,
|
||||
buffer.data(),
|
||||
static_cast<int>(buffer.size()),
|
||||
&transferred,
|
||||
timeout_ms
|
||||
);
|
||||
|
||||
if (transfer_status == LIBUSB_ERROR_TIMEOUT) {
|
||||
return;
|
||||
}
|
||||
if (transfer_status != LIBUSB_SUCCESS) {
|
||||
throw std::runtime_error(libusb_error_message("Failed to read LibreVNA USB bulk packet", transfer_status));
|
||||
}
|
||||
if (transferred <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
rx_buffer_.insert(rx_buffer_.end(), buffer.begin(), buffer.begin() + transferred);
|
||||
decode_frames_from_buffer();
|
||||
}
|
||||
|
||||
void LibreVnaMinimalDriver::decode_frames_from_buffer() {
|
||||
while (true) {
|
||||
const auto header_iter = std::find(rx_buffer_.begin(), rx_buffer_.end(), detail::kFrameHeader);
|
||||
if (header_iter == rx_buffer_.end()) {
|
||||
rx_buffer_.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
if (header_iter != rx_buffer_.begin()) {
|
||||
rx_buffer_.erase(rx_buffer_.begin(), header_iter);
|
||||
}
|
||||
|
||||
if (rx_buffer_.size() < 4U) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto declared_length = detail::read_u16_le(rx_buffer_, 1);
|
||||
if (declared_length < detail::kFrameOverheadBytes || declared_length > detail::kMaxFrameLengthBytes) {
|
||||
rx_buffer_.erase(rx_buffer_.begin());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (rx_buffer_.size() < declared_length) {
|
||||
return;
|
||||
}
|
||||
|
||||
const std::vector<std::uint8_t> frame(rx_buffer_.begin(), rx_buffer_.begin() + declared_length);
|
||||
rx_buffer_.erase(rx_buffer_.begin(), rx_buffer_.begin() + declared_length);
|
||||
|
||||
const auto packet_type = frame[3];
|
||||
const auto received_crc = detail::read_u32_le(frame, frame.size() - 4U);
|
||||
|
||||
if (packet_type == detail::kPacketVnaDatapoint) {
|
||||
if (received_crc != 0U) {
|
||||
throw std::runtime_error("Invalid VNADatapoint CRC (expected zero)");
|
||||
}
|
||||
} else {
|
||||
const auto expected_crc = crc32(std::span<const std::uint8_t>(frame.data(), frame.size() - 4U));
|
||||
if (received_crc != expected_crc) {
|
||||
throw std::runtime_error("Invalid LibreVNA packet CRC");
|
||||
}
|
||||
}
|
||||
|
||||
NativePacket packet{};
|
||||
packet.packet_type = packet_type;
|
||||
packet.payload.assign(frame.begin() + 4, frame.end() - 4);
|
||||
packet_queue_.push_back(std::move(packet));
|
||||
}
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::pop_packet(std::uint8_t packet_type) -> std::optional<NativePacket> {
|
||||
for (auto iter = packet_queue_.begin(); iter != packet_queue_.end(); ++iter) {
|
||||
if (iter->packet_type != packet_type) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto packet = std::move(*iter);
|
||||
packet_queue_.erase(iter);
|
||||
return packet;
|
||||
}
|
||||
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-197
@@ -1,197 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <csignal>
|
||||
#include <signal.h> // POSIX sigaction/NSIG for the chaining stop observer (#26).
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
namespace radar::drivers::librevna::detail {
|
||||
|
||||
constexpr float kPi = 3.14159265358979323846F;
|
||||
|
||||
// Frame format:
|
||||
// [0]=header, [1..2]=length_le, [3]=packet_type, [...payload...], [crc32_le]
|
||||
constexpr std::uint8_t kFrameHeader = 0x5A;
|
||||
constexpr std::size_t kFrameOverheadBytes = 8;
|
||||
constexpr std::size_t kMaxFrameLengthBytes = 4096;
|
||||
|
||||
constexpr unsigned char kEndpointOut = 0x01;
|
||||
constexpr unsigned char kEndpointIn = 0x81;
|
||||
constexpr int kUsbInterface = 0;
|
||||
|
||||
constexpr std::uint8_t kPacketDeviceInfo = 5;
|
||||
constexpr std::uint8_t kPacketAck = 7;
|
||||
constexpr std::uint8_t kPacketNack = 10;
|
||||
constexpr std::uint8_t kPacketRequestDeviceInfo = 15;
|
||||
constexpr std::uint8_t kPacketVnaDatapoint = 27;
|
||||
constexpr std::uint8_t kPacketInitiateSweep = 32;
|
||||
|
||||
constexpr std::size_t kDeviceInfoPayloadSize = 57;
|
||||
constexpr std::size_t kDeviceInfoProtocolOffset = 0;
|
||||
constexpr std::size_t kDeviceInfoNumPortsOffset = 54;
|
||||
constexpr std::uint16_t kProtocolV14 = 14;
|
||||
|
||||
constexpr std::uint8_t kReferenceFlagMask = 0x10;
|
||||
constexpr std::uint8_t kPort1Mask = 0x01;
|
||||
constexpr std::uint8_t kPort2Mask = 0x02;
|
||||
|
||||
constexpr std::array<std::pair<std::uint16_t, std::uint16_t>, 3> kSupportedUsbIds = {
|
||||
std::pair<std::uint16_t, std::uint16_t>{0x0483, 0x564E},
|
||||
std::pair<std::uint16_t, std::uint16_t>{0x0483, 0x4121},
|
||||
std::pair<std::uint16_t, std::uint16_t>{0x1209, 0x4121},
|
||||
};
|
||||
|
||||
constexpr int kUsbWriteTimeoutMs = 500;
|
||||
constexpr int kAckTimeoutMs = 800;
|
||||
constexpr int kDeviceInfoTimeoutSeconds = 2;
|
||||
constexpr std::size_t kUsbReadChunkBytes = 16 * 1024;
|
||||
constexpr int kUsbReadPollMinTimeoutMs = 1;
|
||||
constexpr int kUsbReadPollMaxTimeoutMs = 50;
|
||||
|
||||
// Native sweep timing budget (#7). The native deadline is no longer a single
|
||||
// fixed value for the whole sweep: instead it is derived from the sweep size
|
||||
// (fixed setup cost + per-point dwell ≈ points/IFBW) and is extended every
|
||||
// time a fresh datapoint arrives via a per-gap stall timeout, while an overall
|
||||
// hard cap bounds a wedged device.
|
||||
constexpr int kNativeSweepSetupBudgetMs = 500;
|
||||
constexpr float kNativeSweepDwellMargin = 3.0F;
|
||||
constexpr int kNativeSweepPerGapTimeoutMs = 1500;
|
||||
constexpr int kNativeSweepHardCapMs = 60'000;
|
||||
|
||||
// Reconnect backoff (#31). After a transient error the device may re-enumerate;
|
||||
// retry discovery a few times with a short bounded delay before giving up.
|
||||
constexpr int kNativeDiscoveryRetryAttempts = 10;
|
||||
constexpr int kNativeDiscoveryRetryDelayMs = 150;
|
||||
|
||||
[[nodiscard]] inline auto read_u16_le(std::span<const std::uint8_t> data, std::size_t offset) -> std::uint16_t {
|
||||
if ((offset + sizeof(std::uint16_t)) > data.size()) {
|
||||
throw std::runtime_error("Failed to decode uint16 from payload");
|
||||
}
|
||||
return static_cast<std::uint16_t>(data[offset]) |
|
||||
(static_cast<std::uint16_t>(data[offset + 1]) << 8U);
|
||||
}
|
||||
|
||||
[[nodiscard]] inline auto read_u32_le(std::span<const std::uint8_t> data, std::size_t offset) -> std::uint32_t {
|
||||
if ((offset + sizeof(std::uint32_t)) > data.size()) {
|
||||
throw std::runtime_error("Failed to decode uint32 from payload");
|
||||
}
|
||||
return static_cast<std::uint32_t>(data[offset]) |
|
||||
(static_cast<std::uint32_t>(data[offset + 1]) << 8U) |
|
||||
(static_cast<std::uint32_t>(data[offset + 2]) << 16U) |
|
||||
(static_cast<std::uint32_t>(data[offset + 3]) << 24U);
|
||||
}
|
||||
|
||||
[[nodiscard]] inline auto read_u64_le(std::span<const std::uint8_t> data, std::size_t offset) -> std::uint64_t {
|
||||
if ((offset + sizeof(std::uint64_t)) > data.size()) {
|
||||
throw std::runtime_error("Failed to decode uint64 from payload");
|
||||
}
|
||||
|
||||
std::uint64_t value = 0;
|
||||
for (std::size_t index = 0; index < sizeof(std::uint64_t); ++index) {
|
||||
value |= static_cast<std::uint64_t>(data[offset + index]) << (index * 8U);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
[[nodiscard]] inline auto read_f32_le(std::span<const std::uint8_t> data, std::size_t offset) -> float {
|
||||
const auto raw = read_u32_le(data, offset);
|
||||
float out = 0.0F;
|
||||
std::memcpy(&out, &raw, sizeof(float));
|
||||
return out;
|
||||
}
|
||||
|
||||
inline void write_u16_le(std::span<std::uint8_t> data, std::size_t offset, std::uint16_t value) {
|
||||
if ((offset + sizeof(std::uint16_t)) > data.size()) {
|
||||
throw std::runtime_error("Failed to encode uint16 into frame");
|
||||
}
|
||||
|
||||
data[offset] = static_cast<std::uint8_t>(value & 0xFFU);
|
||||
data[offset + 1] = static_cast<std::uint8_t>((value >> 8U) & 0xFFU);
|
||||
}
|
||||
|
||||
inline void write_u32_le(std::span<std::uint8_t> data, std::size_t offset, std::uint32_t value) {
|
||||
if ((offset + sizeof(std::uint32_t)) > data.size()) {
|
||||
throw std::runtime_error("Failed to encode uint32 into frame");
|
||||
}
|
||||
|
||||
data[offset] = static_cast<std::uint8_t>(value & 0xFFU);
|
||||
data[offset + 1] = static_cast<std::uint8_t>((value >> 8U) & 0xFFU);
|
||||
data[offset + 2] = static_cast<std::uint8_t>((value >> 16U) & 0xFFU);
|
||||
data[offset + 3] = static_cast<std::uint8_t>((value >> 24U) & 0xFFU);
|
||||
}
|
||||
|
||||
// Stop responsiveness (#26). The blocking USB loops keep their per-call
|
||||
// timeouts short and re-poll this flag so a SIGTERM/SIGINT delivered mid-sweep
|
||||
// is honoured within a few hundred ms instead of only after the whole sweep
|
||||
// budget has elapsed.
|
||||
//
|
||||
// `main()` installs its own SIGINT/SIGTERM handlers before any driver is
|
||||
// opened, so we chain to (and preserve) whatever handler is already installed
|
||||
// rather than clobbering it. All state lives in single inline-static slots so
|
||||
// every translation unit observes the same flag and chain table.
|
||||
[[nodiscard]] inline auto native_stop_flag() -> volatile std::sig_atomic_t& {
|
||||
static volatile std::sig_atomic_t flag = 0;
|
||||
return flag;
|
||||
}
|
||||
|
||||
[[nodiscard]] inline auto native_chained_handlers() -> std::array<struct sigaction, NSIG>& {
|
||||
static std::array<struct sigaction, NSIG> handlers{};
|
||||
return handlers;
|
||||
}
|
||||
|
||||
inline void native_stop_signal_handler(int signal_number) {
|
||||
native_stop_flag() = 1;
|
||||
|
||||
// Chain to the previously installed disposition (e.g. main()'s handler) so
|
||||
// process-level stop semantics are unchanged.
|
||||
if (signal_number < 0 || signal_number >= NSIG) {
|
||||
return;
|
||||
}
|
||||
const auto& prior = native_chained_handlers()[static_cast<std::size_t>(signal_number)];
|
||||
if ((prior.sa_flags & SA_SIGINFO) == 0 && prior.sa_handler != nullptr &&
|
||||
prior.sa_handler != SIG_DFL && prior.sa_handler != SIG_IGN &&
|
||||
prior.sa_handler != native_stop_signal_handler) {
|
||||
prior.sa_handler(signal_number);
|
||||
}
|
||||
}
|
||||
|
||||
// Lazily install the chaining stop observer for one signal. Must be called
|
||||
// after main() has installed its handlers (i.e. from open_native()).
|
||||
inline void install_native_stop_observer_for(int signal_number) {
|
||||
if (signal_number < 0 || signal_number >= NSIG) {
|
||||
return;
|
||||
}
|
||||
|
||||
struct sigaction current{};
|
||||
if (sigaction(signal_number, nullptr, ¤t) != 0) {
|
||||
return;
|
||||
}
|
||||
if (current.sa_handler == native_stop_signal_handler) {
|
||||
return; // Already installed; do not chain to ourselves.
|
||||
}
|
||||
|
||||
native_chained_handlers()[static_cast<std::size_t>(signal_number)] = current;
|
||||
|
||||
struct sigaction action{};
|
||||
action.sa_handler = native_stop_signal_handler;
|
||||
sigemptyset(&action.sa_mask);
|
||||
action.sa_flags = current.sa_flags & ~SA_SIGINFO; // Keep flags such as SA_RESTART.
|
||||
sigaction(signal_number, &action, nullptr);
|
||||
}
|
||||
|
||||
inline void install_native_stop_observer() {
|
||||
install_native_stop_observer_for(SIGINT);
|
||||
install_native_stop_observer_for(SIGTERM);
|
||||
}
|
||||
|
||||
[[nodiscard]] inline auto native_stop_requested() -> bool {
|
||||
return native_stop_flag() != 0;
|
||||
}
|
||||
|
||||
} // namespace radar::drivers::librevna::detail
|
||||
|
||||
-112
@@ -1,112 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#if __has_include(<libusb-1.0/libusb.h>)
|
||||
#include <libusb-1.0/libusb.h>
|
||||
#else
|
||||
#include <libusb.h>
|
||||
#endif
|
||||
|
||||
#include "radar_driver.hpp"
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
/**
|
||||
* @brief Runtime settings for minimal LibreVNA acquisition.
|
||||
*/
|
||||
struct LibreVnaMinimalDriverSettings {
|
||||
config::DriverMode mode = config::DriverMode::Mock;
|
||||
std::string serial{};
|
||||
config::RadarSweepSettings sweep{};
|
||||
float mock_signal_hz = 5'000'000.0F;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Minimal radar driver that acquires forward sweeps from LibreVNA.
|
||||
*
|
||||
* This class intentionally keeps scope narrow: open/close transport and
|
||||
* acquire one sweep. Full device configuration is expected to be done by the
|
||||
* Python layer before this process starts.
|
||||
*/
|
||||
class LibreVnaMinimalDriver final : public RadarDriver {
|
||||
public:
|
||||
explicit LibreVnaMinimalDriver(LibreVnaMinimalDriverSettings settings);
|
||||
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto acquire_sweep() -> SweepTrace override;
|
||||
void set_active_combo(const ipc::ComboKey& combo) override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Raw protocol packet decoded from framed USB stream.
|
||||
*/
|
||||
struct NativePacket {
|
||||
std::uint8_t packet_type = 0;
|
||||
std::vector<std::uint8_t> payload{};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief One decoded VNADatapoint containing complex values for one point.
|
||||
*/
|
||||
struct DecodedVnaDatapoint {
|
||||
std::uint32_t point_number = 0;
|
||||
float frequency_hz = 0.0F;
|
||||
ipc::Complex32 s11{};
|
||||
ipc::Complex32 s21{};
|
||||
bool has_s11 = false;
|
||||
bool has_s21 = false;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto acquire_mock() -> SweepTrace;
|
||||
[[nodiscard]] auto acquire_native() -> SweepTrace;
|
||||
|
||||
void open_native();
|
||||
void close_native();
|
||||
|
||||
void send_packet(std::uint8_t packet_type, std::span<const std::uint8_t> payload, bool require_ack);
|
||||
void send_packet_no_payload(std::uint8_t packet_type, bool require_ack);
|
||||
void wait_for_ack(std::chrono::steady_clock::time_point deadline);
|
||||
[[nodiscard]] auto wait_for_packet(std::uint8_t expected_type, std::chrono::steady_clock::time_point deadline)
|
||||
-> NativePacket;
|
||||
void pump_usb(std::chrono::steady_clock::time_point deadline);
|
||||
void decode_frames_from_buffer();
|
||||
|
||||
[[nodiscard]] auto pop_packet(std::uint8_t packet_type) -> std::optional<NativePacket>;
|
||||
[[nodiscard]] static auto encode_frame(std::uint8_t packet_type, std::span<const std::uint8_t> payload)
|
||||
-> std::vector<std::uint8_t>;
|
||||
[[nodiscard]] static auto decode_vna_datapoint_traces(
|
||||
std::span<const std::uint8_t> payload,
|
||||
DecodedVnaDatapoint& datapoint_out
|
||||
) -> bool;
|
||||
[[nodiscard]] static auto crc32(std::span<const std::uint8_t> data) -> std::uint32_t;
|
||||
void validate_device_info_payload(std::span<const std::uint8_t> payload) const;
|
||||
|
||||
LibreVnaMinimalDriverSettings settings_{};
|
||||
bool is_open_ = false;
|
||||
std::uint64_t sweep_index_ = 0;
|
||||
// Latest combo announced by the orchestrator. Used by the mock backend to
|
||||
// give each (input, output) pair a slightly different reflectivity profile
|
||||
// so a multi-combo run does not render as eight identical traces.
|
||||
ipc::ComboKey active_combo_{};
|
||||
|
||||
libusb_context* usb_context_ = nullptr;
|
||||
libusb_device_handle* usb_handle_ = nullptr;
|
||||
bool interface_claimed_ = false;
|
||||
|
||||
std::vector<std::uint8_t> rx_buffer_{};
|
||||
std::deque<NativePacket> packet_queue_{};
|
||||
|
||||
std::uint16_t protocol_version_ = 0;
|
||||
std::uint8_t device_num_ports_ = 0;
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
-271
@@ -1,271 +0,0 @@
|
||||
#include "../remote_compact_m_k209_driver.hpp"
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace {
|
||||
|
||||
constexpr std::uint8_t kCommandConfigure = static_cast<std::uint8_t>('C');
|
||||
constexpr std::uint8_t kCommandAcquire = static_cast<std::uint8_t>('A');
|
||||
constexpr std::uint8_t kStatusOk = static_cast<std::uint8_t>('O');
|
||||
constexpr std::uint8_t kStatusError = static_cast<std::uint8_t>('E');
|
||||
|
||||
[[nodiscard]] auto system_error_message(const std::string& context) -> std::string {
|
||||
return context + ": " + std::strerror(errno);
|
||||
}
|
||||
|
||||
void append_u32_be(std::vector<std::uint8_t>& buffer, std::uint32_t value) {
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 24U) & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 16U) & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 8U) & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>(value & 0xFFU));
|
||||
}
|
||||
|
||||
void append_u64_be(std::vector<std::uint8_t>& buffer, std::uint64_t value) {
|
||||
for (int shift = 56; shift >= 0; shift -= 8) {
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> static_cast<unsigned>(shift)) & 0xFFU));
|
||||
}
|
||||
}
|
||||
|
||||
void append_double_be(std::vector<std::uint8_t>& buffer, double value) {
|
||||
std::uint64_t bits = 0;
|
||||
static_assert(sizeof(bits) == sizeof(value));
|
||||
std::memcpy(&bits, &value, sizeof(bits));
|
||||
append_u64_be(buffer, bits);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_u32_be(const std::uint8_t* data) -> std::uint32_t {
|
||||
return (static_cast<std::uint32_t>(data[0]) << 24U) |
|
||||
(static_cast<std::uint32_t>(data[1]) << 16U) |
|
||||
(static_cast<std::uint32_t>(data[2]) << 8U) |
|
||||
static_cast<std::uint32_t>(data[3]);
|
||||
}
|
||||
|
||||
void send_all(int socket_fd, const void* data, std::size_t size) {
|
||||
const auto* cursor = static_cast<const std::uint8_t*>(data);
|
||||
auto remaining = size;
|
||||
while (remaining > 0U) {
|
||||
const auto sent = ::send(socket_fd, cursor, remaining, MSG_NOSIGNAL);
|
||||
if (sent < 0) {
|
||||
throw std::runtime_error(system_error_message("send to K209 remote server failed"));
|
||||
}
|
||||
if (sent == 0) {
|
||||
throw std::runtime_error("send to K209 remote server returned zero bytes");
|
||||
}
|
||||
cursor += sent;
|
||||
remaining -= static_cast<std::size_t>(sent);
|
||||
}
|
||||
}
|
||||
|
||||
void recv_exact(int socket_fd, void* data, std::size_t size) {
|
||||
auto* cursor = static_cast<std::uint8_t*>(data);
|
||||
auto remaining = size;
|
||||
while (remaining > 0U) {
|
||||
const auto received = ::recv(socket_fd, cursor, remaining, 0);
|
||||
if (received < 0) {
|
||||
throw std::runtime_error(system_error_message("read from K209 remote server failed"));
|
||||
}
|
||||
if (received == 0) {
|
||||
throw std::runtime_error("K209 remote server closed the connection");
|
||||
}
|
||||
cursor += received;
|
||||
remaining -= static_cast<std::size_t>(received);
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto recv_u32(int socket_fd) -> std::uint32_t {
|
||||
std::uint8_t bytes[4]{};
|
||||
recv_exact(socket_fd, bytes, sizeof(bytes));
|
||||
return parse_u32_be(bytes);
|
||||
}
|
||||
|
||||
void read_status(int socket_fd) {
|
||||
std::uint8_t status = 0;
|
||||
recv_exact(socket_fd, &status, 1);
|
||||
if (status == kStatusOk) {
|
||||
return;
|
||||
}
|
||||
if (status == kStatusError) {
|
||||
const auto message_size = recv_u32(socket_fd);
|
||||
std::string message(message_size, '\0');
|
||||
if (message_size > 0U) {
|
||||
recv_exact(socket_fd, message.data(), message.size());
|
||||
}
|
||||
throw std::runtime_error("K209 remote server error: " + message);
|
||||
}
|
||||
throw std::runtime_error("K209 remote server returned invalid status byte");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto recv_float32_array(int socket_fd, std::uint32_t expected_values, const std::string& context)
|
||||
-> std::vector<float> {
|
||||
const auto payload_size = recv_u32(socket_fd);
|
||||
const auto expected_size = expected_values * static_cast<std::uint32_t>(sizeof(float));
|
||||
if (payload_size != expected_size) {
|
||||
throw std::runtime_error(
|
||||
"K209 remote " + context + " payload has " + std::to_string(payload_size) +
|
||||
" bytes, expected " + std::to_string(expected_size)
|
||||
);
|
||||
}
|
||||
|
||||
std::vector<float> values(expected_values);
|
||||
if (payload_size > 0U) {
|
||||
recv_exact(socket_fd, values.data(), payload_size);
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto connect_socket(const std::string& host, std::uint32_t port, std::uint32_t timeout_ms) -> int {
|
||||
addrinfo hints{};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
hints.ai_protocol = IPPROTO_TCP;
|
||||
|
||||
addrinfo* result = nullptr;
|
||||
const auto port_text = std::to_string(port);
|
||||
const auto rc = ::getaddrinfo(host.c_str(), port_text.c_str(), &hints, &result);
|
||||
if (rc != 0) {
|
||||
throw std::runtime_error("getaddrinfo failed for K209 remote host " + host + ": " + ::gai_strerror(rc));
|
||||
}
|
||||
|
||||
int connected_fd = -1;
|
||||
std::string last_error;
|
||||
for (auto* item = result; item != nullptr; item = item->ai_next) {
|
||||
const int fd = ::socket(item->ai_family, item->ai_socktype, item->ai_protocol);
|
||||
if (fd < 0) {
|
||||
last_error = system_error_message("socket");
|
||||
continue;
|
||||
}
|
||||
|
||||
const int one = 1;
|
||||
static_cast<void>(::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)));
|
||||
timeval timeout{};
|
||||
timeout.tv_sec = static_cast<long>(timeout_ms / 1000U);
|
||||
timeout.tv_usec = static_cast<long>((timeout_ms % 1000U) * 1000U);
|
||||
static_cast<void>(::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)));
|
||||
static_cast<void>(::setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)));
|
||||
|
||||
if (::connect(fd, item->ai_addr, item->ai_addrlen) == 0) {
|
||||
connected_fd = fd;
|
||||
break;
|
||||
}
|
||||
last_error = system_error_message("connect");
|
||||
::close(fd);
|
||||
}
|
||||
::freeaddrinfo(result);
|
||||
|
||||
if (connected_fd < 0) {
|
||||
throw std::runtime_error("Failed to connect to K209 remote server " + host + ":" + port_text + ": " + last_error);
|
||||
}
|
||||
return connected_fd;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto complex_from_interleaved(const std::vector<float>& values, std::uint32_t points, const std::string& context)
|
||||
-> std::vector<ipc::Complex32> {
|
||||
if (values.size() != static_cast<std::size_t>(points) * 2U) {
|
||||
throw std::runtime_error("K209 remote " + context + " returned unexpected scalar count");
|
||||
}
|
||||
|
||||
std::vector<ipc::Complex32> output(points);
|
||||
for (std::uint32_t index = 0; index < points; ++index) {
|
||||
output[index] = ipc::Complex32{
|
||||
.re = values[static_cast<std::size_t>(index) * 2U],
|
||||
.im = values[static_cast<std::size_t>(index) * 2U + 1U],
|
||||
};
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
RemoteCompactMK209Driver::RemoteCompactMK209Driver(RemoteCompactMK209DriverSettings settings)
|
||||
: settings_(std::move(settings)) {
|
||||
if (settings_.host.empty()) {
|
||||
throw std::runtime_error("K209 remote host must not be empty");
|
||||
}
|
||||
if (settings_.port == 0U || settings_.port > 65535U) {
|
||||
throw std::runtime_error("K209 remote port must be in 1..65535");
|
||||
}
|
||||
if (settings_.timeout_ms == 0U) {
|
||||
throw std::runtime_error("K209 remote timeout_ms must be > 0");
|
||||
}
|
||||
}
|
||||
|
||||
RemoteCompactMK209Driver::~RemoteCompactMK209Driver() {
|
||||
try {
|
||||
close();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteCompactMK209Driver::open() {
|
||||
if (socket_fd_ >= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
socket_fd_ = connect_socket(settings_.host, settings_.port, settings_.timeout_ms);
|
||||
|
||||
try {
|
||||
std::vector<std::uint8_t> payload{};
|
||||
payload.reserve(1U + 8U + 8U + 4U + 8U + 8U);
|
||||
payload.push_back(kCommandConfigure);
|
||||
append_double_be(payload, static_cast<double>(settings_.sweep.start_hz));
|
||||
append_double_be(payload, static_cast<double>(settings_.sweep.stop_hz));
|
||||
append_u32_be(payload, settings_.sweep.points);
|
||||
append_double_be(payload, static_cast<double>(settings_.sweep.if_bandwidth_hz));
|
||||
append_double_be(payload, static_cast<double>(settings_.sweep.power_dbm));
|
||||
send_all(socket_fd_, payload.data(), payload.size());
|
||||
|
||||
read_status(socket_fd_);
|
||||
const auto points = recv_u32(socket_fd_);
|
||||
if (points != settings_.sweep.points) {
|
||||
throw std::runtime_error("K209 remote server returned unexpected configured point count");
|
||||
}
|
||||
frequency_hz_ = recv_float32_array(socket_fd_, points, "frequency");
|
||||
} catch (...) {
|
||||
close();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteCompactMK209Driver::close() {
|
||||
if (socket_fd_ >= 0) {
|
||||
::close(socket_fd_);
|
||||
socket_fd_ = -1;
|
||||
}
|
||||
frequency_hz_.clear();
|
||||
}
|
||||
|
||||
auto RemoteCompactMK209Driver::acquire_sweep() -> SweepTrace {
|
||||
if (socket_fd_ < 0) {
|
||||
throw std::runtime_error("K209 remote driver is not open");
|
||||
}
|
||||
const std::uint8_t command = kCommandAcquire;
|
||||
send_all(socket_fd_, &command, 1);
|
||||
read_status(socket_fd_);
|
||||
|
||||
const auto points = recv_u32(socket_fd_);
|
||||
if (points != settings_.sweep.points) {
|
||||
throw std::runtime_error("K209 remote server returned unexpected sweep point count");
|
||||
}
|
||||
const auto s11_values = recv_float32_array(socket_fd_, points * 2U, "S11");
|
||||
const auto s21_values = recv_float32_array(socket_fd_, points * 2U, "S21");
|
||||
|
||||
return SweepTrace{
|
||||
.frequency_hz = frequency_hz_,
|
||||
.s11 = complex_from_interleaved(s11_values, points, "S11"),
|
||||
.s21 = complex_from_interleaved(s21_values, points, "S21"),
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-34
@@ -1,34 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "radar_driver.hpp"
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
struct RemoteCompactMK209DriverSettings {
|
||||
std::string host = "127.0.0.1";
|
||||
std::uint32_t port = 50209;
|
||||
config::RadarSweepSettings sweep{};
|
||||
std::uint32_t timeout_ms = 20'000;
|
||||
};
|
||||
|
||||
class RemoteCompactMK209Driver final : public RadarDriver {
|
||||
public:
|
||||
explicit RemoteCompactMK209Driver(RemoteCompactMK209DriverSettings settings);
|
||||
~RemoteCompactMK209Driver() override;
|
||||
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto acquire_sweep() -> SweepTrace override;
|
||||
|
||||
private:
|
||||
RemoteCompactMK209DriverSettings settings_{};
|
||||
int socket_fd_ = -1;
|
||||
std::vector<float> frequency_hz_{};
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
-183
@@ -1,183 +0,0 @@
|
||||
#include "hmc349a_minimal_driver.hpp"
|
||||
|
||||
#include <cerrno>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <linux/gpio.h>
|
||||
#include <stdexcept>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
|
||||
namespace radar::drivers {
|
||||
namespace {
|
||||
|
||||
// A transient ioctl failure (e.g. EINTR/EAGAIN/EBUSY under load) should not be
|
||||
// immediately fatal; retry a small number of times before giving up.
|
||||
constexpr int kSwitchIoctlRetries = 3;
|
||||
|
||||
// Compute the GPIO line values for a given logical position, honouring inverted
|
||||
// control logic. Shared by switch_native() and the teardown default-drive path.
|
||||
[[nodiscard]] auto make_line_values(const HMC349AMinimalDriverSettings& settings, std::uint32_t position)
|
||||
-> gpio_v2_line_values {
|
||||
const std::uint8_t requested_state = static_cast<std::uint8_t>(position & 0x01U);
|
||||
const std::uint8_t control_state = settings.invert_logic ? (requested_state ^ 0x01U) : requested_state;
|
||||
|
||||
gpio_v2_line_values values{};
|
||||
values.mask = (1ULL << 0U);
|
||||
values.bits = static_cast<std::uint64_t>(control_state);
|
||||
return values;
|
||||
}
|
||||
|
||||
void validate_open_settings(const HMC349AMinimalDriverSettings& settings) {
|
||||
if (settings.positions == 0U || settings.positions > 2U) {
|
||||
throw std::runtime_error("HMC349A switch positions must be in range [1, 2] for " + settings.name);
|
||||
}
|
||||
if (settings.default_position >= settings.positions) {
|
||||
throw std::runtime_error("Switch default_position out of range for " + settings.name);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
HMC349AMinimalDriver::HMC349AMinimalDriver(HMC349AMinimalDriverSettings settings)
|
||||
: settings_(std::move(settings)) {}
|
||||
|
||||
HMC349AMinimalDriver::~HMC349AMinimalDriver() {
|
||||
// Safety net: if the owner never called close() (e.g. on crash/unwind),
|
||||
// still drive the RF path to a known-safe state and release the GPIO lines.
|
||||
try {
|
||||
close();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::open() {
|
||||
if (is_open_) {
|
||||
return;
|
||||
}
|
||||
|
||||
validate_open_settings(settings_);
|
||||
|
||||
if (settings_.mode == config::DriverMode::Native) {
|
||||
open_native();
|
||||
}
|
||||
|
||||
is_open_ = true;
|
||||
switch_to(settings_.default_position);
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::close() {
|
||||
if (!is_open_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (settings_.mode == config::DriverMode::Native) {
|
||||
close_native();
|
||||
}
|
||||
|
||||
is_open_ = false;
|
||||
}
|
||||
|
||||
auto HMC349AMinimalDriver::position_count() const -> std::uint32_t {
|
||||
return settings_.positions;
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::switch_to(std::uint32_t position) {
|
||||
if (!is_open_) {
|
||||
throw std::runtime_error("Switch driver is not open for " + settings_.name);
|
||||
}
|
||||
if (position >= settings_.positions) {
|
||||
throw std::runtime_error("Switch position out of range for " + settings_.name);
|
||||
}
|
||||
|
||||
switch (settings_.mode) {
|
||||
case config::DriverMode::Mock:
|
||||
break;
|
||||
case config::DriverMode::Native:
|
||||
switch_native(position);
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error("Unsupported switch driver mode for " + settings_.name);
|
||||
}
|
||||
|
||||
current_position_ = position;
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::open_native() {
|
||||
if (settings_.gpio_chip.empty()) {
|
||||
throw std::runtime_error("gpio_chip is empty for native switch " + settings_.name);
|
||||
}
|
||||
if (settings_.pin_a < 0) {
|
||||
throw std::runtime_error("pin_a (control) is invalid for native switch " + settings_.name);
|
||||
}
|
||||
|
||||
chip_fd_ = ::open(settings_.gpio_chip.c_str(), O_RDONLY | O_CLOEXEC);
|
||||
if (chip_fd_ < 0) {
|
||||
throw std::runtime_error(
|
||||
"Failed to open gpio chip '" + settings_.gpio_chip + "' for " + settings_.name + ": " + std::strerror(errno)
|
||||
);
|
||||
}
|
||||
|
||||
gpio_v2_line_request request{};
|
||||
// Bit 0 controls pin_a.
|
||||
request.offsets[0] = static_cast<std::uint32_t>(settings_.pin_a);
|
||||
request.num_lines = 1;
|
||||
request.config.flags = GPIO_V2_LINE_FLAG_OUTPUT;
|
||||
std::snprintf(request.consumer, sizeof(request.consumer), "radar_%s", settings_.name.c_str());
|
||||
|
||||
if (::ioctl(chip_fd_, GPIO_V2_GET_LINE_IOCTL, &request) != 0) {
|
||||
const auto error = std::strerror(errno);
|
||||
::close(chip_fd_);
|
||||
chip_fd_ = -1;
|
||||
throw std::runtime_error("Failed to request GPIO lines for " + settings_.name + ": " + error);
|
||||
}
|
||||
|
||||
line_fd_ = request.fd;
|
||||
if (line_fd_ < 0) {
|
||||
::close(chip_fd_);
|
||||
chip_fd_ = -1;
|
||||
throw std::runtime_error("GPIO line request returned invalid fd for " + settings_.name);
|
||||
}
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::close_native() {
|
||||
// Leave the RF path in its known-safe default position before releasing the
|
||||
// line, so shutdown/crash never strands the switch in an arbitrary state.
|
||||
if (line_fd_ >= 0 && settings_.default_position < settings_.positions) {
|
||||
auto values = make_line_values(settings_, settings_.default_position);
|
||||
// Best-effort: do not throw out of the teardown path.
|
||||
::ioctl(line_fd_, GPIO_V2_LINE_SET_VALUES_IOCTL, &values);
|
||||
}
|
||||
|
||||
if (line_fd_ >= 0) {
|
||||
::close(line_fd_);
|
||||
line_fd_ = -1;
|
||||
}
|
||||
if (chip_fd_ >= 0) {
|
||||
::close(chip_fd_);
|
||||
chip_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void HMC349AMinimalDriver::switch_native(std::uint32_t position) {
|
||||
if (line_fd_ < 0) {
|
||||
throw std::runtime_error("Native GPIO line fd is not open for " + settings_.name);
|
||||
}
|
||||
|
||||
auto values = make_line_values(settings_, position);
|
||||
|
||||
// Retry transient ioctl failures so a momentary hiccup is recoverable
|
||||
// instead of aborting the whole sweep; only the final attempt is fatal.
|
||||
int last_errno = 0;
|
||||
for (int attempt = 0; attempt < kSwitchIoctlRetries; ++attempt) {
|
||||
if (::ioctl(line_fd_, GPIO_V2_LINE_SET_VALUES_IOCTL, &values) == 0) {
|
||||
return;
|
||||
}
|
||||
last_errno = errno;
|
||||
}
|
||||
throw std::runtime_error("Failed to switch GPIO state for " + settings_.name + ": " + std::strerror(last_errno));
|
||||
}
|
||||
|
||||
} // namespace radar::drivers
|
||||
-53
@@ -1,53 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
#include "run_config.hpp"
|
||||
#include "switch_driver.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
/**
|
||||
* @brief Runtime settings for the HMC349A GPIO switch driver.
|
||||
*/
|
||||
struct HMC349AMinimalDriverSettings {
|
||||
std::string name{};
|
||||
config::DriverMode mode = config::DriverMode::Native;
|
||||
std::uint32_t positions = 2;
|
||||
std::uint32_t default_position = 0;
|
||||
std::string gpio_chip = "/dev/gpiochip0";
|
||||
std::int32_t pin_a = 17; ///< Control pin.
|
||||
bool invert_logic = false; ///< Invert control logic for position mapping.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Minimal HMC349A implementation for deterministic GPIO control.
|
||||
*
|
||||
* Native mode uses Linux GPIO character-device API (uAPI v2).
|
||||
* Mock mode keeps state in memory without touching hardware.
|
||||
*/
|
||||
class HMC349AMinimalDriver final : public SwitchDriver {
|
||||
public:
|
||||
explicit HMC349AMinimalDriver(HMC349AMinimalDriverSettings settings);
|
||||
~HMC349AMinimalDriver() override; // Leaves RF path safe even if close() was skipped.
|
||||
|
||||
void open() override;
|
||||
void close() override;
|
||||
[[nodiscard]] auto position_count() const -> std::uint32_t override;
|
||||
void switch_to(std::uint32_t position) override;
|
||||
|
||||
private:
|
||||
void open_native();
|
||||
void close_native();
|
||||
void switch_native(std::uint32_t position);
|
||||
|
||||
HMC349AMinimalDriverSettings settings_{};
|
||||
bool is_open_ = false;
|
||||
std::uint32_t current_position_ = 0;
|
||||
|
||||
int chip_fd_ = -1;
|
||||
int line_fd_ = -1;
|
||||
};
|
||||
|
||||
} // namespace radar::drivers
|
||||
@@ -1,60 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
|
||||
#include "radar_driver.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
#include "switch_driver.hpp"
|
||||
|
||||
namespace radar::acq {
|
||||
|
||||
/**
|
||||
* @brief Produces raw sweep collections for configured switch combinations.
|
||||
*
|
||||
* Lifecycle:
|
||||
* 1. Open radar and switch drivers.
|
||||
* 2. Iterate configured combinations, acquire one sweep per combination.
|
||||
* 3. Publish serialized collections into the raw shared-memory ring.
|
||||
*/
|
||||
class SweepOrchestrator {
|
||||
public:
|
||||
/**
|
||||
* @param config Runtime config loaded from JSON.
|
||||
* @param radar_driver Radar device implementation.
|
||||
* @param input_switch_driver Switch connected to radar input path.
|
||||
* @param output_switch_driver Switch connected to radar output path.
|
||||
* @param raw_ring Shared-memory ring for raw collections.
|
||||
* @param raw_tap_ring Optional tap ring for GUI/debug readers.
|
||||
*/
|
||||
SweepOrchestrator(
|
||||
const config::RunConfig& config,
|
||||
drivers::RadarDriver& radar_driver,
|
||||
drivers::SwitchDriver& input_switch_driver,
|
||||
drivers::SwitchDriver& output_switch_driver,
|
||||
ipc::ShmRing& raw_ring,
|
||||
ipc::ShmRing* raw_tap_ring = nullptr
|
||||
);
|
||||
|
||||
/**
|
||||
* @brief Run acquisition loop until stop is requested or single run completes.
|
||||
*/
|
||||
void run(const std::atomic<bool>& stop_requested);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Acquire one collection for all configured combinations.
|
||||
*/
|
||||
[[nodiscard]] auto acquire_one_collection(std::uint64_t collection_id, const std::atomic<bool>& stop_requested)
|
||||
-> ipc::RawSweepCollection;
|
||||
|
||||
const config::RunConfig& config_;
|
||||
drivers::RadarDriver& radar_driver_;
|
||||
drivers::SwitchDriver& input_switch_driver_;
|
||||
drivers::SwitchDriver& output_switch_driver_;
|
||||
ipc::ShmRing& raw_ring_;
|
||||
ipc::ShmRing* raw_tap_ring_ = nullptr;
|
||||
};
|
||||
|
||||
} // namespace radar::acq
|
||||
@@ -1,169 +0,0 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "h7992_minimal_driver.hpp"
|
||||
#include "hmc349a_minimal_driver.hpp"
|
||||
#include "librevna_minimal_driver.hpp"
|
||||
#include "radar_driver.hpp"
|
||||
#include "remote_compact_m_k209_driver.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
#include "sweep_orchestrator.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kDefaultConfigPath = "run_config.json";
|
||||
constexpr const char* kLibreVnaModel = "librevna";
|
||||
constexpr const char* kLibreVnaMultiModel = "librevna_multi";
|
||||
constexpr const char* kCompactMK209Model = "compact_m_k209";
|
||||
std::atomic<bool> g_stop_requested{false};
|
||||
|
||||
void signal_handler(int /*signal*/) {
|
||||
g_stop_requested.store(true, std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
void install_signal_handlers() {
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_config_path(int argc, char** argv) -> std::string {
|
||||
std::string config_path = kDefaultConfigPath;
|
||||
|
||||
for (int index = 1; index < argc; ++index) {
|
||||
const std::string arg = argv[index];
|
||||
if (arg == "--config" && (index + 1) < argc) {
|
||||
config_path = argv[++index];
|
||||
}
|
||||
}
|
||||
|
||||
return config_path;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_radar_driver(const radar::config::RadarConfig& config)
|
||||
-> std::unique_ptr<radar::drivers::RadarDriver> {
|
||||
if (config.model == kLibreVnaModel || config.model.empty()) {
|
||||
return std::make_unique<radar::drivers::LibreVnaMinimalDriver>(
|
||||
radar::drivers::LibreVnaMinimalDriverSettings{
|
||||
.mode = config.driver_mode,
|
||||
.serial = config.serial,
|
||||
.sweep = config.sweep,
|
||||
.mock_signal_hz = config.mock_signal_hz,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
if (config.model == kCompactMK209Model) {
|
||||
if (config.driver_mode != radar::config::DriverMode::Native) {
|
||||
throw std::runtime_error("compact_m_k209 requires radar.driver_mode='native'");
|
||||
}
|
||||
return std::make_unique<radar::drivers::RemoteCompactMK209Driver>(
|
||||
radar::drivers::RemoteCompactMK209DriverSettings{
|
||||
.host = config.remote_host,
|
||||
.port = config.remote_port,
|
||||
.sweep = config.sweep,
|
||||
.timeout_ms = 20'000,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
if (config.model == kLibreVnaMultiModel) {
|
||||
throw std::runtime_error("librevna_multi is handled by python_app.scripts.matrix_raw_producer");
|
||||
}
|
||||
|
||||
if (config.model == "sn9000") {
|
||||
throw std::runtime_error("sn9000 is handled by python_app.scripts.matrix_raw_producer");
|
||||
}
|
||||
|
||||
throw std::runtime_error("Unsupported radar.model for sweep_orchestrator: " + config.model);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_h7992_driver(const radar::config::SwitchConfig& config)
|
||||
-> std::unique_ptr<radar::drivers::SwitchDriver> {
|
||||
return std::make_unique<radar::drivers::H7992MinimalDriver>(
|
||||
radar::drivers::H7992MinimalDriverSettings{
|
||||
.name = config.name,
|
||||
.mode = config.driver_mode,
|
||||
.positions = config.positions,
|
||||
.default_position = config.default_position,
|
||||
.gpio_chip = config.gpio_chip,
|
||||
.pin_a = config.pin_a,
|
||||
.pin_b = config.pin_b,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_hmc349a_driver(const radar::config::SwitchConfig& config)
|
||||
-> std::unique_ptr<radar::drivers::SwitchDriver> {
|
||||
return std::make_unique<radar::drivers::HMC349AMinimalDriver>(
|
||||
radar::drivers::HMC349AMinimalDriverSettings{
|
||||
.name = config.name,
|
||||
.mode = config.driver_mode,
|
||||
.positions = config.positions,
|
||||
.default_position = config.default_position,
|
||||
.gpio_chip = config.gpio_chip,
|
||||
.pin_a = config.pin_a,
|
||||
.invert_logic = config.invert_logic,
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_switch_driver(const radar::config::SwitchConfig& config)
|
||||
-> std::unique_ptr<radar::drivers::SwitchDriver> {
|
||||
using radar::config::SwitchDriverKind;
|
||||
|
||||
switch (config.driver_kind) {
|
||||
case SwitchDriverKind::H7992:
|
||||
return make_h7992_driver(config);
|
||||
case SwitchDriverKind::HMC349A:
|
||||
return make_hmc349a_driver(config);
|
||||
default:
|
||||
throw std::runtime_error("Unsupported switch driver kind");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
install_signal_handlers();
|
||||
|
||||
try {
|
||||
const auto config_path = read_config_path(argc, argv);
|
||||
const auto config = radar::config::load_run_config(config_path);
|
||||
|
||||
auto raw_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.raw.name,
|
||||
config.rings.raw.capacity,
|
||||
config.rings.raw.slot_size_bytes
|
||||
);
|
||||
auto raw_tap_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.raw_tap.name,
|
||||
config.rings.raw_tap.capacity,
|
||||
config.rings.raw_tap.slot_size_bytes
|
||||
);
|
||||
|
||||
auto radar_driver = make_radar_driver(config.radar);
|
||||
|
||||
auto input_switch = make_switch_driver(config.input_switch);
|
||||
auto output_switch = make_switch_driver(config.output_switch);
|
||||
|
||||
radar::acq::SweepOrchestrator orchestrator(
|
||||
config,
|
||||
*radar_driver,
|
||||
*input_switch,
|
||||
*output_switch,
|
||||
raw_ring,
|
||||
&raw_tap_ring
|
||||
);
|
||||
orchestrator.run(g_stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "sweep_orchestrator error: " << exception.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -1,328 +0,0 @@
|
||||
#include "sweep_orchestrator.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::acq {
|
||||
namespace {
|
||||
|
||||
// Wait-for-device retry tuning (mirror of python matrix_raw_producer._open_radar_with_retry):
|
||||
// a device that is absent at boot or disappears mid-run must never kill the orchestrator,
|
||||
// only make it wait. Backoff is capped so a long absence does not busy-spin, and every wait
|
||||
// is interruptible by stop_requested for a prompt clean exit.
|
||||
constexpr std::uint32_t kOpenRetryMinMs = 1'000U;
|
||||
constexpr std::uint32_t kOpenRetryMaxMs = 10'000U;
|
||||
// Throttle open-failure logging during a long wait so a permanently absent device does not
|
||||
// flood the process log: log the first failure, then every Nth attempt.
|
||||
constexpr std::uint64_t kOpenRetryLogEvery = 30U;
|
||||
|
||||
[[nodiscard]] inline auto should_stop(const std::atomic<bool>& stop_requested) -> bool {
|
||||
return stop_requested.load(std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
void sleep_if_needed_ms(std::uint32_t delay_ms) {
|
||||
if (delay_ms == 0U) {
|
||||
return;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
}
|
||||
|
||||
// Interruptible sleep: returns true if a stop was requested while waiting.
|
||||
[[nodiscard]] auto interruptible_sleep_ms(std::uint32_t delay_ms, const std::atomic<bool>& stop_requested) -> bool {
|
||||
constexpr std::uint32_t kPollMs = 50U;
|
||||
std::uint32_t waited = 0U;
|
||||
while (waited < delay_ms) {
|
||||
if (should_stop(stop_requested)) {
|
||||
return true;
|
||||
}
|
||||
const std::uint32_t chunk = std::min(kPollMs, delay_ms - waited);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(chunk));
|
||||
waited += chunk;
|
||||
}
|
||||
return should_stop(stop_requested);
|
||||
}
|
||||
|
||||
void validate_sweep(const drivers::SweepTrace& sweep) {
|
||||
if (sweep.frequency_hz.size() != sweep.s11.size() || sweep.frequency_hz.size() != sweep.s21.size()) {
|
||||
throw std::runtime_error("Radar driver returned inconsistent sweep vectors");
|
||||
}
|
||||
}
|
||||
|
||||
class DriverLifecycleGuard {
|
||||
public:
|
||||
DriverLifecycleGuard(
|
||||
drivers::RadarDriver& radar_driver,
|
||||
drivers::SwitchDriver& input_switch_driver,
|
||||
drivers::SwitchDriver& output_switch_driver
|
||||
)
|
||||
: radar_driver_(radar_driver),
|
||||
input_switch_driver_(input_switch_driver),
|
||||
output_switch_driver_(output_switch_driver) {}
|
||||
|
||||
~DriverLifecycleGuard() noexcept {
|
||||
if (!active_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Shutdown should never hide the primary error path.
|
||||
try {
|
||||
output_switch_driver_.close();
|
||||
} catch (...) {
|
||||
}
|
||||
try {
|
||||
input_switch_driver_.close();
|
||||
} catch (...) {
|
||||
}
|
||||
try {
|
||||
radar_driver_.close();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void open_all() {
|
||||
radar_driver_.open();
|
||||
input_switch_driver_.open();
|
||||
output_switch_driver_.open();
|
||||
active_ = true;
|
||||
}
|
||||
|
||||
// Open all devices, retrying forever with capped exponential backoff until success or
|
||||
// stop. Used for both the initial open and every in-loop reopen, so a device that is
|
||||
// absent at boot or disappears mid-run never kills the orchestrator — it just waits.
|
||||
// Returns false only if a stop was requested before any device became available.
|
||||
[[nodiscard]] auto open_all_with_retry(const std::atomic<bool>& stop_requested) -> bool {
|
||||
// Drop any partial state from a previous open before retrying.
|
||||
close_all();
|
||||
|
||||
std::uint64_t attempt = 0;
|
||||
std::uint32_t delay_ms = kOpenRetryMinMs;
|
||||
while (!should_stop(stop_requested)) {
|
||||
try {
|
||||
open_all();
|
||||
if (attempt > 0) {
|
||||
std::cerr << "sweep_orchestrator: devices opened after " << (attempt + 1) << " attempt(s)\n";
|
||||
}
|
||||
return true;
|
||||
} catch (const std::exception& exc) {
|
||||
// Best-effort: drop any partial open before the next attempt.
|
||||
close_all();
|
||||
++attempt;
|
||||
if (attempt == 1 || attempt % kOpenRetryLogEvery == 0) {
|
||||
std::cerr << "sweep_orchestrator: devices not available (attempt " << attempt
|
||||
<< "); retrying up to every " << (kOpenRetryMaxMs / 1'000U)
|
||||
<< "s until present: " << exc.what() << '\n';
|
||||
}
|
||||
if (interruptible_sleep_ms(delay_ms, stop_requested)) {
|
||||
return false;
|
||||
}
|
||||
delay_ms = std::min(delay_ms * 2U, kOpenRetryMaxMs);
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void close_all() {
|
||||
if (!active_) {
|
||||
return;
|
||||
}
|
||||
output_switch_driver_.close();
|
||||
input_switch_driver_.close();
|
||||
radar_driver_.close();
|
||||
active_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
drivers::RadarDriver& radar_driver_;
|
||||
drivers::SwitchDriver& input_switch_driver_;
|
||||
drivers::SwitchDriver& output_switch_driver_;
|
||||
bool active_ = false;
|
||||
};
|
||||
|
||||
// Worst-case serialized size of a collection given the configured combo count and sweep
|
||||
// point count, using the trace wire format (see ipc::write_trace_collection/write_trace_block):
|
||||
// collection header: magic(4) + collection_id(8) + monotonic_ns(8) + trace_count(4)
|
||||
// + capture_start_ns(8) + capture_end_ns(8)
|
||||
// + trace capture window count(4) = 44 bytes
|
||||
// per trace block: input_pos(4) + output_pos(4) + point_count(4) = 12 bytes
|
||||
// + per point: frequency(4) + s11(8) + s21(8) = 20 bytes
|
||||
// + trailer: capture_start_ns(8) + capture_end_ns(8) = 16 bytes
|
||||
[[nodiscard]] auto worst_case_serialized_bytes(std::size_t combo_count, std::uint32_t sweep_points) -> std::size_t {
|
||||
constexpr std::size_t kCollectionHeaderBytes = 44U;
|
||||
constexpr std::size_t kTraceHeaderBytes = 12U;
|
||||
constexpr std::size_t kBytesPerPoint = 20U;
|
||||
constexpr std::size_t kTraceTrailerBytes = 16U;
|
||||
const std::size_t per_trace =
|
||||
kTraceHeaderBytes + (static_cast<std::size_t>(sweep_points) * kBytesPerPoint) + kTraceTrailerBytes;
|
||||
return kCollectionHeaderBytes + (combo_count * per_trace);
|
||||
}
|
||||
|
||||
// Publish a collection on the primary raw ring; tap pushes are strictly best-effort.
|
||||
// Never throws: an oversize payload is logged-and-dropped (the slot-size budget is also
|
||||
// validated up front at startup, so this guards only against unexpected runtime growth).
|
||||
// Returns true if the primary push succeeded.
|
||||
[[nodiscard]] auto publish_collection(
|
||||
ipc::ShmRing& raw_ring,
|
||||
ipc::ShmRing* raw_tap_ring,
|
||||
const ipc::RawSweepCollection& collection,
|
||||
std::uint64_t& oversize_drop_count
|
||||
) -> bool {
|
||||
const auto serialized_collection = ipc::serialize_raw_collection(collection);
|
||||
if (!raw_ring.push(serialized_collection)) {
|
||||
// Log-and-drop an oversize payload rather than aborting the long-running loop.
|
||||
// Throttle so a persistently oversize payload cannot flood the log.
|
||||
if (oversize_drop_count % 100 == 0) {
|
||||
std::cerr << "sweep_orchestrator: dropped oversize raw payload (" << serialized_collection.size()
|
||||
<< " bytes > slot " << raw_ring.slot_size_bytes() << "; drop count="
|
||||
<< (oversize_drop_count + 1) << ")\n";
|
||||
}
|
||||
++oversize_drop_count;
|
||||
return false;
|
||||
}
|
||||
// Tap ring is for GUI/debug observers only: a tap failure must never abort the primary path.
|
||||
if (raw_tap_ring != nullptr) {
|
||||
(void)raw_tap_ring->push(serialized_collection);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
SweepOrchestrator::SweepOrchestrator(
|
||||
const config::RunConfig& config,
|
||||
drivers::RadarDriver& radar_driver,
|
||||
drivers::SwitchDriver& input_switch_driver,
|
||||
drivers::SwitchDriver& output_switch_driver,
|
||||
ipc::ShmRing& raw_ring,
|
||||
ipc::ShmRing* raw_tap_ring
|
||||
)
|
||||
: config_(config),
|
||||
radar_driver_(radar_driver),
|
||||
input_switch_driver_(input_switch_driver),
|
||||
output_switch_driver_(output_switch_driver),
|
||||
raw_ring_(raw_ring),
|
||||
raw_tap_ring_(raw_tap_ring) {}
|
||||
|
||||
void SweepOrchestrator::run(const std::atomic<bool>& stop_requested) {
|
||||
// Fail fast on a config error: a slot that is too small for the worst-case payload can
|
||||
// never carry a full collection, so report it clearly at startup instead of dropping
|
||||
// every collection at runtime.
|
||||
const auto worst_case_bytes = worst_case_serialized_bytes(config_.run_combos.size(), config_.radar.sweep.points);
|
||||
if (worst_case_bytes > raw_ring_.slot_size_bytes()) {
|
||||
throw std::runtime_error(
|
||||
"Raw ring slot_size_bytes (" + std::to_string(raw_ring_.slot_size_bytes())
|
||||
+ ") is too small for the worst-case serialized collection (" + std::to_string(worst_case_bytes)
|
||||
+ " bytes for " + std::to_string(config_.run_combos.size()) + " combos x "
|
||||
+ std::to_string(config_.radar.sweep.points) + " points)"
|
||||
);
|
||||
}
|
||||
|
||||
DriverLifecycleGuard lifecycle_guard(radar_driver_, input_switch_driver_, output_switch_driver_);
|
||||
// Wait for the devices to become available before starting: an absent device
|
||||
// makes the orchestrator wait, not exit.
|
||||
if (!lifecycle_guard.open_all_with_retry(stop_requested)) {
|
||||
return; // stop requested before any device became available
|
||||
}
|
||||
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t oversize_drop_count = 0;
|
||||
while (!should_stop(stop_requested)) {
|
||||
try {
|
||||
auto raw_collection = acquire_one_collection(++collection_id, stop_requested);
|
||||
if (raw_collection.traces.empty()) {
|
||||
if (!config_.runtime.continuous) {
|
||||
break;
|
||||
}
|
||||
sleep_if_needed_ms(config_.runtime.idle_sleep_ms);
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)publish_collection(raw_ring_, raw_tap_ring_, raw_collection, oversize_drop_count);
|
||||
|
||||
if (!config_.runtime.continuous) {
|
||||
break;
|
||||
}
|
||||
} catch (const std::exception& exc) {
|
||||
// Treat any in-loop device error (timeout/NACK/transient USB or socket glitch/
|
||||
// device-not-found-after-glitch) as recoverable: close, wait for the device to
|
||||
// come back, and continue. This mirrors the python producer's reconnect-forever
|
||||
// policy so a long-running headless appliance survives transient hardware hiccups
|
||||
// instead of exiting. Genuinely fatal config errors are caught at startup above
|
||||
// (and in main()), reserving non-zero exit for those.
|
||||
std::cerr << "sweep_orchestrator: acquisition failed; reopening and waiting for the device: "
|
||||
<< exc.what() << '\n';
|
||||
if (!config_.runtime.continuous) {
|
||||
throw; // single-run mode has no recovery path; surface the failure
|
||||
}
|
||||
if (!lifecycle_guard.open_all_with_retry(stop_requested)) {
|
||||
break; // stop requested while waiting to reopen
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
lifecycle_guard.close_all();
|
||||
}
|
||||
|
||||
auto SweepOrchestrator::acquire_one_collection(
|
||||
std::uint64_t collection_id,
|
||||
const std::atomic<bool>& stop_requested
|
||||
) -> ipc::RawSweepCollection {
|
||||
ipc::RawSweepCollection collection{};
|
||||
collection.collection_id = collection_id;
|
||||
collection.monotonic_ns = ipc::current_monotonic_ns();
|
||||
collection.traces.reserve(config_.run_combos.size());
|
||||
collection.capture_start_ns = ipc::current_monotonic_ns();
|
||||
bool interrupted = false;
|
||||
|
||||
for (const auto& combo : config_.run_combos) {
|
||||
if (should_stop(stop_requested)) {
|
||||
interrupted = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// Hardware flow for each combo:
|
||||
// 1) switch output path, 2) switch input path, 3) optional settle.
|
||||
output_switch_driver_.switch_to(combo.output_pos);
|
||||
input_switch_driver_.switch_to(combo.input_pos);
|
||||
sleep_if_needed_ms(config_.runtime.settling_ms);
|
||||
|
||||
// Production drivers ignore this; mock drivers use it to give every
|
||||
// (input, output) pair its own synthetic response.
|
||||
radar_driver_.set_active_combo(combo);
|
||||
// Stamp around the sweep only: the switch drive and settling above belong to
|
||||
// neither the previous combo nor this one, so excluding them keeps the window
|
||||
// an honest "when was this combo actually measured".
|
||||
const auto sweep_start_ns = ipc::current_monotonic_ns();
|
||||
auto sweep = radar_driver_.acquire_sweep();
|
||||
const auto sweep_end_ns = ipc::current_monotonic_ns();
|
||||
validate_sweep(sweep);
|
||||
|
||||
ipc::SweepTraceBlock trace{};
|
||||
trace.combo = combo;
|
||||
trace.frequency_hz = std::move(sweep.frequency_hz);
|
||||
trace.s11 = std::move(sweep.s11);
|
||||
trace.s21 = std::move(sweep.s21);
|
||||
trace.capture_start_ns = sweep_start_ns;
|
||||
trace.capture_end_ns = sweep_end_ns;
|
||||
collection.traces.push_back(std::move(trace));
|
||||
}
|
||||
|
||||
if (interrupted) {
|
||||
// Do not emit partial collections when stop was requested mid-cycle.
|
||||
collection.traces.clear();
|
||||
collection.capture_start_ns = 0;
|
||||
collection.capture_end_ns = 0;
|
||||
return collection;
|
||||
}
|
||||
|
||||
collection.capture_end_ns = collection.traces.empty() ? 0U : ipc::current_monotonic_ns();
|
||||
|
||||
return collection;
|
||||
}
|
||||
|
||||
} // namespace radar::acq
|
||||
@@ -0,0 +1,154 @@
|
||||
#include "sar3d_processor.hpp"
|
||||
#include "gpr_processor.hpp"
|
||||
#include "../processing/processors/src/sar3d_clean.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <complex>
|
||||
#include <iostream>
|
||||
#include <numbers>
|
||||
#include <vector>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
using Json = nlohmann::json;
|
||||
using namespace radar;
|
||||
|
||||
namespace {
|
||||
auto complex_samples(const Json& samples) -> std::vector<std::complex<float>> {
|
||||
std::vector<std::complex<float>> values;
|
||||
for (const auto& sample : samples) {
|
||||
values.emplace_back(sample.at(0).get<float>(), sample.at(1).get<float>());
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
auto collection(const Json& source) -> ipc::PreprocessedCollection {
|
||||
ipc::PreprocessedCollection result;
|
||||
result.collection_id = source.at("id");
|
||||
result.monotonic_ns = source.at("timestamp");
|
||||
for (const auto& entry : source.at("traces")) {
|
||||
ipc::SweepTraceBlock trace;
|
||||
trace.combo = {entry.at("input"), entry.at("output")};
|
||||
trace.capture_start_ns = entry.at("start");
|
||||
trace.capture_end_ns = entry.at("end");
|
||||
trace.frequency_hz = entry.at("frequency").get<std::vector<float>>();
|
||||
for (const auto& sample : complex_samples(entry.at("s21"))) {
|
||||
trace.s21.push_back({sample.real(), sample.imag()});
|
||||
}
|
||||
if (result.traces.empty() || trace.capture_start_ns < result.capture_start_ns) {
|
||||
result.capture_start_ns = trace.capture_start_ns;
|
||||
}
|
||||
result.capture_end_ns = std::max(result.capture_end_ns, trace.capture_end_ns);
|
||||
result.traces.push_back(std::move(trace));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// Boxes around given CLEAN objects, rows [x, y, z, y_low, y_high, level], in a scene with the
|
||||
// given X and Y bounds and radar placement (sar3d::marks).
|
||||
auto marks(const Json& input) -> Json {
|
||||
std::vector<processing::sar3d::CleanObject> objects;
|
||||
for (const auto& row : input.at("objects")) {
|
||||
objects.push_back({.index = {}, .position = {row.at(0), row.at(1), row.at(2)},
|
||||
.y_low = row.at(3), .y_high = row.at(4), .level = row.at(5)});
|
||||
}
|
||||
const auto x = input.at("x").get<std::vector<double>>();
|
||||
const auto y = input.at("y").get<std::vector<double>>();
|
||||
const processing::sar3d::VolumeRequest frame{
|
||||
.grid = {x, y, {}},
|
||||
.pitch_rad = input.at("pitch").get<double>() * (std::numbers::pi / 180.0),
|
||||
.ground_depth_m = input.at("ground"),
|
||||
.min_depth_m = input.at("min_depth"),
|
||||
.max_depth_m = input.at("max_depth"),
|
||||
};
|
||||
Json rows = Json::array();
|
||||
for (const auto& mark : processing::sar3d::marks(objects, frame)) {
|
||||
Json row = Json::array({static_cast<int>(mark.kind)});
|
||||
for (const double edge : mark.box) { row.push_back(edge); }
|
||||
rows.push_back(std::move(row));
|
||||
}
|
||||
return Json{{"marks", rows}};
|
||||
}
|
||||
}
|
||||
|
||||
// Runs GPR or sar3d on one request or a batch, retaining processor caches. Each request holds the antenna
|
||||
// geometry, the live settings and the latest and previous collections; the
|
||||
// published payloads go to stdout as JSON. A request with mode "marks" boxes given objects instead.
|
||||
int main() {
|
||||
try {
|
||||
const Json source = Json::parse(std::cin);
|
||||
const bool batch = source.is_array();
|
||||
const Json requests = batch ? source : Json::array({source});
|
||||
Json results = Json::array();
|
||||
processing::Sar3dProcessor sar;
|
||||
processing::GprProcessor gpr;
|
||||
for (const auto& input : requests) {
|
||||
if (input.value("mode", "sar3d") == "marks") {
|
||||
results.push_back(marks(input));
|
||||
continue;
|
||||
}
|
||||
config::RunConfig config;
|
||||
config.rings.results.slot_size_bytes = input.value("slot", config.rings.results.slot_size_bytes);
|
||||
for (const auto& row : input.at("tx")) {
|
||||
config.gpr.tx_geometry.push_back({row.at(0), row.at(1), row.at(2), row.at(3)});
|
||||
}
|
||||
for (const auto& row : input.at("rx")) {
|
||||
config.gpr.rx_geometry.push_back({row.at(0), row.at(1), row.at(2), row.at(3)});
|
||||
}
|
||||
const bool gpr_mode = input.value("mode", "sar3d") == "gpr";
|
||||
processing::ProcessingLiveConfig live;
|
||||
live.gpr_start_freq_mhz = input.at("f_start").get<double>() / 1e6;
|
||||
live.gpr_stop_freq_mhz = input.at("f_stop").get<double>() / 1e6;
|
||||
live.gpr_min_depth_m = input.at("min_depth");
|
||||
live.gpr_max_depth_m = input.at("max_depth");
|
||||
live.gpr_speed_m_s = input.at("speed");
|
||||
live.gpr_look_angle_deg = input.at("pitch");
|
||||
live.pair_depth_equalize = input.value("equalize", true);
|
||||
live.gpr_background_subtract_enabled = input.value("subtract_mean", false);
|
||||
live.gpr_background_mean_count = input.value("mean_count", 10U);
|
||||
if (gpr_mode) {
|
||||
live.gpr_bp_ground_depth_m = input.at("ground");
|
||||
live.gpr_range_comp_power = input.at("power");
|
||||
live.gpr_motion_mode = input.value("motion", "int_minus");
|
||||
live.gpr_object_approach_min_frames = 1;
|
||||
live.gpr_remove_sidelobe_objects_enabled = false;
|
||||
} else {
|
||||
const auto& voxel = input.at("voxel");
|
||||
live.sar3d_voxel_x_m = voxel.at(0);
|
||||
live.sar3d_voxel_y_m = voxel.at(1);
|
||||
live.sar3d_voxel_z_m = voxel.at(2);
|
||||
live.sar3d_bp_ground_depth_m = input.at("ground");
|
||||
live.sar3d_frame_radius = input.at("radius");
|
||||
live.sar3d_comp_range_power = input.at("power");
|
||||
live.sar3d_use_compensation = input.at("compensation");
|
||||
live.sar3d_normalize_each_frame = input.at("normalize");
|
||||
live.sar3d_x_size_m = input.value("x_size", 0.0F);
|
||||
live.sar3d_y_min_m = input.value("y_min", -0.5F);
|
||||
live.sar3d_y_max_m = input.value("y_max", 2.0F);
|
||||
live.sar3d_z_size_m = input.value("z_size", 0.0F);
|
||||
live.sar3d_bg_gate_enabled = input.value("bg_gate", live.sar3d_bg_gate_enabled);
|
||||
live.sar3d_clean_enabled = input.value("clean", live.sar3d_clean_enabled);
|
||||
}
|
||||
std::vector<ipc::PreprocessedCollection> previous;
|
||||
for (const auto& member : input.at("previous")) {
|
||||
previous.push_back(collection(member));
|
||||
}
|
||||
const auto latest = collection(input.at("latest"));
|
||||
for (const auto& trace : latest.traces) { config.run_combos.push_back(trace.combo); }
|
||||
processing::ProcessorInterface& processor = gpr_mode
|
||||
? static_cast<processing::ProcessorInterface&>(gpr) : static_cast<processing::ProcessorInterface&>(sar);
|
||||
const auto result = processor.process_collection(config, latest, previous, live);
|
||||
Json payloads = Json::array();
|
||||
for (const auto& payload : result.collection_payloads) {
|
||||
payloads.push_back({{"name", payload.processing_name},
|
||||
{"x", payload.image_x_axis}, {"y", payload.image_y_axis},
|
||||
{"image", payload.image_values}, {"table", payload.table_values},
|
||||
{"columns", payload.table_columns}});
|
||||
}
|
||||
results.push_back(Json{{"payloads", payloads}});
|
||||
}
|
||||
std::cout << (batch ? results : results.at(0)) << '\n';
|
||||
} catch (const std::exception& error) {
|
||||
std::cout << Json{{"error", error.what()}} << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
// Exercise the production collector with a test-only SDK and a real PTY.
|
||||
#define main collector_program_main
|
||||
#include "../rph_radar_collector/src/main.cpp"
|
||||
#undef main
|
||||
#include <sys/wait.h>
|
||||
#include <fcntl.h>
|
||||
#include <fstream>
|
||||
#include <cstring>
|
||||
#include <filesystem>
|
||||
|
||||
|
||||
void test_source_session_restart() {
|
||||
char directory[] = "/tmp/rph-collector-XXXXXX";
|
||||
if (!::mkdtemp(directory)) { fail("mkdtemp failed"); }
|
||||
const std::string tty = std::string(directory) + "/tty", log = std::string(directory) + "/sdk.log";
|
||||
const auto child = ::fork();
|
||||
if (child == 0) {
|
||||
::setenv("RPH_SDK_LOG", log.c_str(), 1);
|
||||
Config cfg;
|
||||
cfg.tty_path = tty;
|
||||
cfg.switching = Json::parse(R"({"input":{"name":"input","driver_mode":"mock","positions":2,"default_position":0,"gpio_chip":"/dev/unused","pin_a":1,"pin_b":2},"output":{"name":"output","driver_mode":"mock","positions":2,"default_position":0,"gpio_chip":"/dev/unused","pin_a":3,"pin_b":4},"combos":[[0,0],[0,1]],"settling_ms":0})");
|
||||
try { ::_exit(run(cfg)); }
|
||||
catch (const std::exception& error) { std::cerr << error.what() << '\n'; ::_exit(2); }
|
||||
}
|
||||
if (child < 0) { std::filesystem::remove_all(directory); fail("collector fork failed"); }
|
||||
int fd = -1, status = 0;
|
||||
std::vector<unsigned char> packets;
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
|
||||
bool finished = false;
|
||||
const auto drain = [&]() {
|
||||
unsigned char bytes[4096];
|
||||
while (fd >= 0) {
|
||||
const auto count = ::read(fd, bytes, sizeof(bytes));
|
||||
if (count <= 0) { break; }
|
||||
packets.insert(packets.end(), bytes, bytes + count);
|
||||
}
|
||||
};
|
||||
while (!finished && std::chrono::steady_clock::now() < deadline) {
|
||||
if (fd < 0) { fd = ::open(tty.c_str(), O_RDONLY | O_NONBLOCK); }
|
||||
drain();
|
||||
finished = ::waitpid(child, &status, WNOHANG) == child;
|
||||
if (!finished) { ::usleep(1000); }
|
||||
}
|
||||
drain();
|
||||
if (fd >= 0) { ::close(fd); }
|
||||
if (!finished) { ::kill(child, SIGKILL); (void)::waitpid(child, &status, 0); }
|
||||
std::ifstream file(log);
|
||||
const std::string calls{std::istreambuf_iterator<char>(file), std::istreambuf_iterator<char>()};
|
||||
std::filesystem::remove_all(directory);
|
||||
if (!finished || !WIFEXITED(status) || WEXITSTATUS(status)) { fail("SDK session regression did not finish successfully"); }
|
||||
const std::string session = "Create\nOpen\nStreamsStop\nStreamsDisable\nConfigure\nOutCycleLoadStart\nPrepareData\nSend\nOutCycleSetup WAIT_DONE\nStart\nOutCycleStop WAIT_DONE\nStreamsStop\nStreamsDisable\nClose\nFree\n";
|
||||
if (calls != session + session + session + session) { fail("SDK session/DOUT restart order changed: " + calls); }
|
||||
const auto read_le = [&packets](std::size_t offset, unsigned size) {
|
||||
uint64_t value = 0;
|
||||
for (unsigned i = 0; i < size; ++i) { value |= uint64_t{packets.at(offset + i)} << (8 * i); }
|
||||
return value;
|
||||
};
|
||||
unsigned clean = 0, total = 0;
|
||||
for (std::size_t offset = 0; offset < packets.size();) {
|
||||
if (packets.size() - offset < 48 || std::memcmp(packets.data() + offset, "RPH1", 4)) { fail("Invalid SDK fixture packet"); }
|
||||
const auto payload = read_le(offset + 4, 4), dirty = read_le(offset + 44, 4);
|
||||
if (payload != 16 || offset + 48 + payload > packets.size()) { fail("Incomplete phase sweep from SDK fixture"); }
|
||||
++total;
|
||||
if (!dirty) {
|
||||
++clean;
|
||||
if (read_le(offset + 8, 8) != 2 * clean || read_le(offset + 16, 8) != (clean + 1) / 2 ||
|
||||
read_le(offset + 40, 2) != 0 || read_le(offset + 42, 2) != (clean - 1) % 2 ||
|
||||
static_cast<int16_t>(read_le(offset + 52, 2)) != pack_raw_code_to_int16(10000 * clean)) {
|
||||
fail("Old ADC block crossed GPIO change, or source/cycle identity reset");
|
||||
}
|
||||
}
|
||||
offset += 48 + payload;
|
||||
}
|
||||
if (clean != 3 || total != 6) { fail("Expected only new-session full sweeps and dirty initial partials"); }
|
||||
}
|
||||
|
||||
int main() {
|
||||
try {
|
||||
test_source_session_restart();
|
||||
for (const auto* period : {"do8_cycle_period:262145", "do8_cycle_period:4294967295"}) {
|
||||
std::vector<std::string> args{"collector", "profile:phase", "clock:internal", "tty:/tmp/unused",
|
||||
"do1_toggle_per_frame", "do1_pair_subtract_avg", "do8_freq_ref", period};
|
||||
std::vector<char*> argv;
|
||||
for (auto& arg : args) { argv.push_back(arg.data()); }
|
||||
bool rejected = false;
|
||||
try { (void)parse_args(static_cast<int>(argv.size()), argv.data()); }
|
||||
catch (const std::runtime_error& error) {
|
||||
rejected = std::string(error.what()).find("cyclic pattern exceeds") != std::string::npos;
|
||||
}
|
||||
if (!rejected) { throw std::runtime_error("DO8 overflow must be rejected before allocation or SDK access"); }
|
||||
}
|
||||
// Preserve the application's low-minus-high IQ averaging and quantization.
|
||||
Do1PairSubtractAverageState state;
|
||||
state.start_run(false);
|
||||
state.add_pair_sample(false, 1000, -3000);
|
||||
state.add_pair_sample(false, 5000, 1000);
|
||||
state.add_pair_sample(true, 200, -1000);
|
||||
state.add_pair_sample(true, 1000, -1000);
|
||||
if (state.diff_count != 2 || state.avg_diff_ch1() != 2400 || state.avg_diff_ch2() != 0 ||
|
||||
pack_raw_code_to_int16(X502_ADC_SCALE_CODE_MAX) != 32767 ||
|
||||
pack_raw_code_to_int16(-2 * X502_ADC_SCALE_CODE_MAX) != -32768) {
|
||||
throw std::runtime_error("phase IQ subtraction/averaging changed");
|
||||
}
|
||||
// An SDK teardown can hang or ignore SIGTERM; parent death must still
|
||||
// release the ADC. Reap the orphan here so the regression leaks nothing.
|
||||
if (::prctl(PR_SET_CHILD_SUBREAPER, 1) != 0) { throw std::runtime_error("subreaper failed"); }
|
||||
int ready[2], identity[2];
|
||||
if (::pipe(ready) != 0 || ::pipe(identity) != 0) { throw std::runtime_error("pipe failed"); }
|
||||
const auto owner = ::fork();
|
||||
if (owner == 0) {
|
||||
const auto collector = ::fork();
|
||||
if (collector < 0) { ::_exit(2); }
|
||||
if (collector == 0) {
|
||||
::close(ready[0]);
|
||||
::signal(SIGTERM, SIG_IGN);
|
||||
bind_to_parent();
|
||||
const char started = 1;
|
||||
if (::write(ready[1], &started, 1) != 1) { ::_exit(2); }
|
||||
for (;;) { ::pause(); }
|
||||
}
|
||||
::close(ready[1]);
|
||||
if (::write(identity[1], &collector, sizeof(collector)) != sizeof(collector)) { ::_exit(2); }
|
||||
char started = 0;
|
||||
::_exit(::read(ready[0], &started, 1) == 1 ? 0 : 2);
|
||||
}
|
||||
if (owner < 0) { throw std::runtime_error("fork failed"); }
|
||||
::close(ready[0]);
|
||||
::close(ready[1]);
|
||||
::close(identity[1]);
|
||||
int status = 0;
|
||||
(void)::waitpid(owner, &status, 0);
|
||||
pid_t child = 0;
|
||||
if (::read(identity[0], &child, sizeof(child)) != sizeof(child)) { throw std::runtime_error("missing child PID"); }
|
||||
::close(identity[0]);
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(1);
|
||||
pid_t orphan = 0;
|
||||
while ((orphan = ::waitpid(-1, &status, WNOHANG)) == 0 && std::chrono::steady_clock::now() < deadline) {
|
||||
::usleep(1000);
|
||||
}
|
||||
if (orphan <= 0 || !WIFSIGNALED(status) || WTERMSIG(status) != SIGKILL) {
|
||||
if (child > 0) { ::kill(child, SIGKILL); (void)::waitpid(child, &status, 0); }
|
||||
throw std::runtime_error("collector survived parent death or entered SIGTERM teardown");
|
||||
}
|
||||
std::cout << "collector regressions passed\n";
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << error.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,415 @@
|
||||
"""Generate SAR3D/GPR references from notebook cells 1/5 and Horns coherent BP.
|
||||
|
||||
Requires the original, untracked Multi_depthnorm_clean.ipynb; a missing notebook
|
||||
is an error. NumPy/SciPy execute its function/class ASTs unchanged. Only parameter
|
||||
values are replaced; plotting imports and the interactive top-level runs are
|
||||
excluded. No production SAR implementation or hand-transcribed oracle is used.
|
||||
|
||||
Run from the repository root:
|
||||
.venv/bin/python data_acq_and_processing/tests/generate_backprojection_reference.py
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import ast
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
import tempfile
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import scipy
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
REFERENCE = Path(__file__).with_name("sar3d_reference.npz")
|
||||
|
||||
|
||||
def reference_functions(source_path: Path, parameters: dict, cells: tuple[int, ...] = (1, 5)) -> tuple[dict, dict]:
|
||||
contents = source_path.read_text(encoding="utf-8")
|
||||
sources = [(str(i), "".join(json.loads(contents)["cells"][i]["source"])) for i in cells] if cells else [("script", contents)]
|
||||
namespace = {"__name__": __name__}
|
||||
hashes = {}
|
||||
for index, source in sources:
|
||||
hashes[str(index)] = hashlib.sha256(source.encode()).hexdigest()
|
||||
tree = ast.parse(source, filename=f"{source_path.name}:cell{index}")
|
||||
definitions = []
|
||||
for node in tree.body:
|
||||
if isinstance(node, (ast.FunctionDef, ast.ClassDef, ast.ImportFrom)):
|
||||
definitions.append(node)
|
||||
elif isinstance(node, ast.Import):
|
||||
if not any(alias.name.startswith(("matplotlib", "plotly")) for alias in node.names):
|
||||
definitions.append(node)
|
||||
elif isinstance(node, ast.Assign) and len(node.targets) == 1 and isinstance(node.targets[0], ast.Name):
|
||||
try:
|
||||
namespace[node.targets[0].id] = ast.literal_eval(node.value)
|
||||
except (ValueError, TypeError):
|
||||
pass # Dataset loading and the interactive run are supplied below.
|
||||
namespace.update(parameters)
|
||||
exec(compile(ast.Module(definitions, type_ignores=[]), f"{source_path.name}:cell{index}",
|
||||
"exec", dont_inherit=True), namespace)
|
||||
return namespace, hashes
|
||||
|
||||
|
||||
def scene(f_start=1e9, f_stop=4e9, points=201) -> dict:
|
||||
"""Deterministic two-echo input with motion, vertical baselines and four pairs."""
|
||||
request = dict(
|
||||
tx=[[0, -.4, 0, 0], [1, .45, .2, .05]],
|
||||
rx=[[0, -.2, .1, 0], [1, .3, .35, .02]], f_start=f_start, f_stop=f_stop,
|
||||
min_depth=1., max_depth=3., speed=.4, pitch=13., ground=.3,
|
||||
voxel=[.4, .4, .4], radius=2, power=1.5, compensation=True, normalize=True,
|
||||
equalize=True, subtract_mean=False, mean_count=1, motion="int_minus",
|
||||
bg_gate=True, clean=False,
|
||||
)
|
||||
# Match the float32 settings/geometry accepted by the native fixture.
|
||||
for key in ("tx", "rx"):
|
||||
request[key] = np.asarray(request[key], dtype=np.float32).astype(float).tolist()
|
||||
frequency = np.linspace(f_start, f_stop, points, dtype=np.float32)
|
||||
members = []
|
||||
for frame, offset in enumerate((0, 180_000_000, 510_000_000)):
|
||||
traces = []
|
||||
start = 10_000_000_000 + offset
|
||||
for tx in range(2):
|
||||
for rx in range(2):
|
||||
samples = ((1 + .1 * rx) * np.exp(-2j * np.pi * frequency.astype(float)
|
||||
* (4.8 + .08 * tx - .03 * frame) / 3e8)
|
||||
+ .35 * np.exp(-2j * np.pi * frequency.astype(float) * (5.6 - .05 * rx) / 3e8)).astype(np.complex64)
|
||||
traces.append(dict(input=rx, output=tx, start=start, end=start + 165_000_000,
|
||||
frequency=frequency.tolist(), s21=np.column_stack((samples.real, samples.imag)).tolist()))
|
||||
members.append(dict(id=frame + 1, timestamp=start + 165_000_000, traces=traces))
|
||||
request.update(previous=members[:-1], latest=members[-1])
|
||||
return request
|
||||
|
||||
|
||||
# Point reflectors (X, height Y, Z, amplitude) at heights of 0, 0.6 and 0.25 m.
|
||||
REFLECTORS = [(.3, 0., 2.2, 1.), (-.8, .6, 2.6, .5), (1.3, .25, 1.9, .7)]
|
||||
|
||||
|
||||
def point_scene(targets: list[tuple[float, float, float, float]], noise: float, seed: int, **settings) -> dict:
|
||||
"""Settings of one static frame of point reflectors (X, height Y, Z, amplitude) in noise, for CLEAN.
|
||||
|
||||
Three Tx and three Rx at different heights resolve Y in a single frame. The voxel steps
|
||||
keep the notebook's proportions, 2.5 : 2 : 2.
|
||||
"""
|
||||
request = dict(
|
||||
tx=[[0, -.6, 0., 0.], [1, 0., .35, .02], [2, .6, .1, 0.]],
|
||||
rx=[[0, -.3, .2, 0.], [1, .3, .45, .03], [2, .9, 0., .01]],
|
||||
min_depth=1., max_depth=3.5, speed=0., pitch=13., ground=1., voxel=[.0625, .05, .05], radius=0,
|
||||
clean=True, previous=[],
|
||||
)
|
||||
request.update(settings)
|
||||
for key in ("tx", "rx"):
|
||||
request[key] = np.asarray(request[key], dtype=np.float32).astype(float).tolist()
|
||||
frequency = np.linspace(1e9, 4e9, 201, dtype=np.float32)
|
||||
cos_p, sin_p = np.cos(np.radians(request["pitch"])), np.sin(np.radians(request["pitch"]))
|
||||
random = np.random.default_rng(seed)
|
||||
traces = []
|
||||
for tx in request["tx"]:
|
||||
for rx in request["rx"]:
|
||||
samples = noise * (random.standard_normal(frequency.size) + 1j * random.standard_normal(frequency.size))
|
||||
for x, height, z, amplitude in targets:
|
||||
# Scene to radar axes, as sar_scene_to_radar_yz.
|
||||
y0 = height - request["ground"]
|
||||
point = np.array([x, y0 * cos_p + z * sin_p, -y0 * sin_p + z * cos_p])
|
||||
path = np.linalg.norm(point - tx[1:]) + np.linalg.norm(point - rx[1:])
|
||||
samples += amplitude * np.exp(-2j * np.pi * frequency.astype(float) * path / 3e8)
|
||||
samples = samples.astype(np.complex64)
|
||||
traces.append(dict(input=int(rx[0]), output=int(tx[0]), start=10_000_000_000, end=10_165_000_000,
|
||||
frequency=frequency.tolist(), s21=np.column_stack((samples.real, samples.imag)).tolist()))
|
||||
request["latest"] = dict(id=1, timestamp=10_165_000_000, traces=traces)
|
||||
return request
|
||||
|
||||
|
||||
def write_dataset(directory: Path, request: dict) -> list[Path]:
|
||||
"""Supply the notebook's original config_profile/meta/NPY input contract."""
|
||||
profile = {"gpr": {}, "run": {"combos": []}}
|
||||
for kind, position in (("tx", "output_pos"), ("rx", "input_pos")):
|
||||
profile["gpr"][f"{kind}_geometry"] = [
|
||||
{position: int(index), "x_m": x, "y_m": y, "z_m": z}
|
||||
for index, x, y, z in request[kind]
|
||||
]
|
||||
profile["run"]["combos"] = [
|
||||
{"input": trace["input"], "output": trace["output"]} for trace in request["latest"]["traces"]
|
||||
]
|
||||
(directory / "config_profile.json").write_text(json.dumps(profile), encoding="utf-8")
|
||||
snapshots = []
|
||||
members = [*request["previous"], request["latest"]]
|
||||
for index, member in enumerate(members):
|
||||
background = previous_mean({**request, "previous": members[:index]})
|
||||
snapshot = directory / f'{index:04d}_id{member["id"]}_ns{member["timestamp"]}'
|
||||
snapshot.mkdir()
|
||||
metadata = {"traces": []}
|
||||
for trace in member["traces"]:
|
||||
stem = f'i{trace["input"]}_o{trace["output"]}'
|
||||
values = np.asarray(trace["s21"])
|
||||
samples = values[:, 0] + 1j * values[:, 1] - background.get((trace["output"], trace["input"]), 0.)
|
||||
np.save(snapshot / f"{stem}_s21.npy", samples)
|
||||
np.save(snapshot / f"{stem}_freq.npy", np.asarray(trace["frequency"], dtype=np.float32))
|
||||
metadata["traces"].append(dict(input=trace["input"], output=trace["output"],
|
||||
capture_start_ns=trace["start"], capture_end_ns=trace["end"]))
|
||||
(snapshot / "meta.json").write_text(json.dumps(metadata), encoding="utf-8")
|
||||
snapshots.append(snapshot)
|
||||
return snapshots
|
||||
|
||||
|
||||
def reference(notebook: Path, request: dict, directory: Path) -> tuple[dict, dict]:
|
||||
snapshots = write_dataset(directory, request)
|
||||
center = snapshots[-1]
|
||||
parameters = dict(
|
||||
SAR_Y_RANGE_M=(request.get("y_min", -.5), request.get("y_max", 2.)),
|
||||
BG_GATE_ENABLE=request["bg_gate"], SAR_CLEAN_ENABLE=request["clean"], SAR_MARK_ENABLE=True,
|
||||
INPUT_IDX=[int(row[0]) for row in request["rx"]],
|
||||
OUTPUT_IDX=[int(row[0]) for row in request["tx"]],
|
||||
F_START=request["f_start"], F_STOP=request["f_stop"],
|
||||
MIN_DEPTH=request["min_depth"], MAX_DEPTH=request["max_depth"],
|
||||
PAIR_NORM_DEPTH_MIN=request["min_depth"], PAIR_NORM_DEPTH_MAX=request["max_depth"],
|
||||
SPEED_M_S=request["speed"], LOOK_ANGLE_DEG=request["pitch"],
|
||||
BP_GROUND_DEPTH_M=request["ground"], COMP_RANGE_POWER=request["power"],
|
||||
SAR_VOXEL_M=tuple(request["voxel"]), SAR_FRAME_RADIUS=request["radius"],
|
||||
SAR_USE_COMPENSATION=request["compensation"], SAR_NORMALIZE_EACH_FRAME=request["normalize"],
|
||||
PAIR_DEPTH_EQUALIZE=request["equalize"],
|
||||
SAR_SCAN_ROOT=directory, DATA_PATH=center, BG_PATH=directory, v=3e8,
|
||||
)
|
||||
ns, hashes = reference_functions(notebook, parameters)
|
||||
tx, rx, order, tx_order, pair_count = ns["load_antenna_config_profile"](
|
||||
directory / "config_profile.json", ns["INPUT_IDX"], ns["OUTPUT_IDX"])
|
||||
ns["N_tx"], ns["N_rx"] = len(tx), len(rx)
|
||||
for kind, positions in (("tx", tx), ("rx", rx)):
|
||||
for axis, values in zip("xyz", ns["positions_to_xyz"](positions).T):
|
||||
ns[f"{axis}_{kind}"] = values
|
||||
antenna_x = np.concatenate((ns["x_tx"], ns["x_rx"]))
|
||||
ns["x_ant"] = antenna_x
|
||||
ns["x_min"], ns["x_max"] = antenna_x.min() - 2, antenna_x.max() + 2
|
||||
ns["x_grid_bp"] = np.linspace(ns["x_min"], ns["x_max"], ns["NX_BP"])
|
||||
sweep_time, _, _ = ns["estimate_sweep_time_from_meta"](directory, n_pairs=pair_count)
|
||||
config = ns["MotionConfig"](
|
||||
speed_m_s=ns["SPEED_M_S"], look_angle_deg=ns["LOOK_ANGLE_DEG"],
|
||||
tx_sweep_time_s=sweep_time, frame_acq_time_s=sweep_time * pair_count,
|
||||
pair_order_phys=order, tx_order_full=tuple(tx_order),
|
||||
n_freq_hint=len(request["latest"]["traces"][0]["frequency"]),
|
||||
apply_pair_motion_correction=ns["APPLY_PAIR_MOTION_CORRECTION"],
|
||||
apply_freq_phase_correction=ns["APPLY_FREQ_PHASE_CORRECTION"],
|
||||
pair_motion_correction_mode=ns["PAIR_MOTION_CORRECTION_MODE"],
|
||||
motion_phase_sign=ns["MOTION_PHASE_SIGN"],
|
||||
)
|
||||
timestamps, _ = ns["compute_pair_timestamps_for_snapshot"](center, config)
|
||||
grid = ns["sar_build_full_region_grid"](config)
|
||||
for index, axis in ((0, "x"), (2, "z")):
|
||||
size, step = request.get(f"{axis}_size", 0.), request["voxel"][index]
|
||||
if size:
|
||||
center_axis = .5 * (grid[axis][0] + grid[axis][-1])
|
||||
grid[axis] = np.linspace(center_axis - size/2, center_axis + size/2, max(round(size/step)+1, 2))
|
||||
grid["XX"], grid["ZZ"] = np.meshgrid(grid["x"], grid["z"])
|
||||
ns["x_min"], ns["x_max"] = grid["x"][0], grid["x"][-1]
|
||||
result = ns["sar_compute_volume"](center, None, config, timestamps, grid=grid, verbose=False)
|
||||
# What the processor publishes: the frame volume, or the CLEAN display when CLEAN found
|
||||
# objects, and the peak it marks; the background gain stays to the GUI.
|
||||
ns["sar_prepare_display"](result, None, timestamps)
|
||||
objects = result["clean_objects"] or []
|
||||
volume, grid = result["display_volume"] if objects else result["volume"], result["grid"]
|
||||
iy, iz, ix = result["display_max_idx"]
|
||||
expected = {axis: grid[axis] for axis in "xyz"}
|
||||
expected.update(front=volume.max(axis=1), side=volume.max(axis=2), top=volume.max(axis=0),
|
||||
peak=np.array([result["level"], grid["x"][ix], grid["y"][iy], grid["z"][iz]]))
|
||||
if not objects:
|
||||
# Width of the response through the marked maximum; objects are marked instead.
|
||||
from scipy.signal import peak_widths
|
||||
widths = []
|
||||
for axis, peak_index, profile in (("x", ix, volume[iy, iz, :]), ("y", iy, volume[:, iz, ix]),
|
||||
("z", iz, volume[iy, :, ix])):
|
||||
profile = np.asarray(profile, dtype=np.float64)
|
||||
width, *_ = peak_widths(profile, [peak_index], prominence_data=(np.array([profile[peak_index]]),
|
||||
np.array([0], dtype=np.intp), np.array([len(profile)-1], dtype=np.intp)))
|
||||
widths.append(max(float(width[0]), 1.) * (grid[axis][1]-grid[axis][0]))
|
||||
expected["target_width"] = np.asarray(widths)
|
||||
if request["bg_gate"]:
|
||||
expected["background"] = np.array([result["bg_ratio"], ns["BG_GATE_3D_SHOW"], ns["BG_GATE_3D_FULL"]])
|
||||
if result["clean_info"] is not None:
|
||||
expected["clean"] = np.array([result["clean_info"]["steps"], result["clean_info"]["residual_ratio"]])
|
||||
if objects:
|
||||
kinds = {"low": 0, "ground": 1, "up": 2, "edge": 3}
|
||||
expected["objects"] = np.array([[item["x"], item["y"], item["z"], item["level_bg"]] for item in objects])
|
||||
expected["marks"] = np.array([[kinds[group["kind"]], *group["box"]] for group in result["mark_groups"]])
|
||||
return expected, hashes
|
||||
|
||||
|
||||
def marks_reference(notebook: Path) -> tuple[dict, np.ndarray]:
|
||||
"""sar_mark_groups on synthetic CLEAN objects, each of which tests one rule of the boxes.
|
||||
|
||||
Returns the fixture's "marks" request and the expected rows [class, X0, X1, Y0, Y1, Z0, Z1].
|
||||
"""
|
||||
scene = dict(x=[-2.6, 2.9], y=[-.5, 2.], pitch=13., ground=1., min_depth=1., max_depth=3.5)
|
||||
objects = [ # [x, y, z, y_low, y_high, level], strongest first
|
||||
[0., 0., 2., -.1, .3, 60.], # low
|
||||
[1., .05, 2.2, -.1, .6, 55.], # centre low, top above SAR_MARK_LOW_TOP_M: ground
|
||||
[-1., .3, 2.4, .1, .5, 50.], # ground
|
||||
[-.2, .45, 3., .25, .65, 45.], # up while its weak member (last) stays out of the centre
|
||||
[-1.6, 0., 1.3, -.1, .3, 40.], # a chain of three: the ends are 1.4 m apart, still one box
|
||||
[-.9, .1, 1.3, -.1, .35, 35.],
|
||||
[-.2, .05, 1.3, -.15, .3, 30.],
|
||||
[-2.5, .2, 2., 0., .4, 28.], # near the X minimum: edge
|
||||
[2.75, .2, 2., 0., .4, 26.], # near the X maximum: edge
|
||||
[2.2, .1, 3.2, -.1, .3, 24.], # near MAX_DEPTH: edge
|
||||
[2.2, 0., .85, -.2, .2, 22.], # near MIN_DEPTH: edge
|
||||
[1.2, 1.7, 2.8, 1.5, 1.9, 20.], # near the top: edge
|
||||
[.2, -.4, 3.1, -.5, -.25, 15.], # weak member of the "up" box, below 40 % of its top
|
||||
]
|
||||
ns, _ = reference_functions(notebook, dict(BP_GROUND_DEPTH_M=scene["ground"], MIN_DEPTH=scene["min_depth"],
|
||||
MAX_DEPTH=scene["max_depth"]))
|
||||
groups = ns["sar_mark_groups"](
|
||||
[dict(zip(("x", "y", "z", "y_lo", "y_hi", "level_bg"), row)) for row in objects],
|
||||
{axis: np.asarray(scene[axis]) for axis in "xy"}, SimpleNamespace(look_angle_deg=scene["pitch"]))
|
||||
kinds = {"low": 0, "ground": 1, "up": 2, "edge": 3}
|
||||
return dict(mode="marks", objects=objects, **scene), np.array([[kinds[g["kind"]], *g["box"]] for g in groups])
|
||||
|
||||
|
||||
def previous_mean(request: dict) -> dict:
|
||||
samples = {}
|
||||
if request["subtract_mean"] and request["mean_count"]:
|
||||
for member in request["previous"][-request["mean_count"]:]:
|
||||
for trace in member["traces"]:
|
||||
values = np.asarray(trace["s21"])
|
||||
samples.setdefault((trace["output"], trace["input"]), []).append(values[:, 0] + 1j * values[:, 1])
|
||||
return {key: np.mean(values, axis=0) for key, values in samples.items()}
|
||||
|
||||
|
||||
def gpr_reference(notebook: Path, script: Path, request: dict) -> tuple[np.ndarray, dict]:
|
||||
"""Execute Horns coherent BP unchanged, adding the notebook's equalizer before its scalar norm."""
|
||||
parameters = dict(F_START=request["f_start"], F_STOP=request["f_stop"], MIN_DEPTH=request["min_depth"],
|
||||
MAX_DEPTH=request["max_depth"], LOOK_ANGLE_DEG=request["pitch"],
|
||||
BP_GROUND_DEPTH_M=request["ground"], COMP_RANGE_POWER=request["power"], v=3e8)
|
||||
ns, hashes = reference_functions(script, parameters, ())
|
||||
ns["N_tx"], ns["N_rx"] = len(request["tx"]), len(request["rx"])
|
||||
ns["OUTPUT_IDX"], ns["INPUT_IDX"] = [int(row[0]) for row in request["tx"]], [int(row[0]) for row in request["rx"]]
|
||||
for kind in ("tx", "rx"):
|
||||
for axis, values in zip("xyz", np.asarray(request[kind])[:, 1:].T):
|
||||
ns[f"{axis}_{kind}"] = values
|
||||
xs = np.concatenate((ns["x_tx"], ns["x_rx"]))
|
||||
ns["x_ant"] = xs
|
||||
ns["x_min"], ns["x_max"] = xs.min()-2, xs.max()+2
|
||||
ns["x_grid_bp"] = np.linspace(ns["x_min"], ns["x_max"], ns["NX_BP"])
|
||||
ns["z_grid_bp"] = np.linspace(.1, request["max_depth"], ns["NZ_BP"])
|
||||
ns["XX_bp"], ns["ZZ_bp"] = np.meshgrid(ns["x_grid_bp"], ns["z_grid_bp"])
|
||||
ns["depth_gate"] = (ns["ZZ_bp"] >= request["min_depth"]) & (ns["ZZ_bp"] <= request["max_depth"])
|
||||
member = request["latest"]
|
||||
duration = (max(t["end"] for t in member["traces"]) - min(t["start"] for t in member["traces"])) * 1e-9
|
||||
config = ns["MotionConfig"](speed_m_s=request["speed"], look_angle_deg=request["pitch"],
|
||||
tx_per_segment=len(request["tx"]), tx_order_full=tuple(ns["OUTPUT_IDX"]),
|
||||
frame_acq_time_s=duration, n_freq_hint=len(member["traces"][0]["frequency"]),
|
||||
intra_phase_focus_only=request["motion"] == "int_focus", motion_phase_sign=-1.,
|
||||
pair_order_phys=[(t["output"], t["input"]) for t in member["traces"]])
|
||||
clocks, _ = ns["compute_pair_timestamps"](config, ns["INPUT_IDX"], ns["OUTPUT_IDX"])
|
||||
mean = previous_mean(request)
|
||||
H, T = {}, {}
|
||||
for trace in member["traces"]:
|
||||
key = (trace["output"], trace["input"])
|
||||
values = np.asarray(trace["s21"], dtype=np.float32)
|
||||
samples = values[:, 0].astype(float) + 1j * values[:, 1].astype(float)
|
||||
samples -= mean.get(key, 0.)
|
||||
frequency = np.asarray(trace["frequency"], dtype=np.float32)
|
||||
samples, _ = ns["apply_intra_sweep_phase_correction"](samples, frequency, clocks[key], config,
|
||||
ns["v"], ns["F_START"], ns["F_STOP"])
|
||||
t, _, h, _, _ = ns["compute_ascan_bp"](samples, frequency, ns["F_START"], ns["F_STOP"])
|
||||
H[key], T[key] = h, t
|
||||
if request["equalize"]:
|
||||
eq, notebook_hashes = reference_functions(notebook, {**parameters, "PAIR_DEPTH_EQUALIZE": True})
|
||||
hashes.update({f"notebook_cell_{cell}": value for cell, value in notebook_hashes.items()})
|
||||
eq.update(x_ant=xs, x_min=ns["x_min"], x_max=ns["x_max"])
|
||||
H = eq["equalize_pairs_by_depth"](H, T)
|
||||
for key, h in H.items():
|
||||
depth = T[key] * ns["v"] / 2
|
||||
mask = (depth >= request["min_depth"]) & (depth <= request["max_depth"])
|
||||
scale = np.percentile(np.abs(h)[mask] if mask.any() else np.abs(h), 50.)
|
||||
H[key] = h / ((scale if np.isfinite(scale) and scale > 1e-15 else 1.) + 1e-15)
|
||||
raw, _ = ns["backproject_coherent"](H, T)
|
||||
return ns["normalize_bp_map"](raw).astype(np.float32), hashes
|
||||
|
||||
|
||||
def generate_gpr(notebook: Path, script: Path, output: Path) -> None:
|
||||
cases = [dict(equalize=False), dict(equalize=True),
|
||||
dict(equalize=True, subtract_mean=True, mean_count=1, speed=-.2, pitch=-7., motion="int_focus", power=2.)]
|
||||
arrays = {}
|
||||
for i, settings in enumerate(cases):
|
||||
request = scene(1.01e9, 4.01e9, 151) if i == 1 else scene()
|
||||
request.update(settings, mode="gpr")
|
||||
arrays[f"case_{i}_image"], hashes = gpr_reference(notebook, script, request)
|
||||
arrays[f"case_{i}_request"] = np.array(json.dumps(request))
|
||||
arrays["provenance"] = np.array(json.dumps(dict(script=script.name, source_sha256=hashes,
|
||||
notebook=notebook.name, numpy=np.__version__, scipy=scipy.__version__)))
|
||||
np.savez_compressed(output, **arrays)
|
||||
print(f"Wrote {output}: {output.stat().st_size} bytes, {len(cases)} GPR references")
|
||||
|
||||
|
||||
def generate(notebook: Path, output: Path) -> None:
|
||||
# Read up front so a missing notebook cannot produce an incomplete artifact.
|
||||
notebook.read_text(encoding="utf-8")
|
||||
cases = [
|
||||
dict(compensation=True, normalize=True),
|
||||
dict(compensation=True, normalize=False, equalize=False, speed=-.2, pitch=-7., ground=.5),
|
||||
# Three different voxel steps: each axis keeps its own.
|
||||
dict(compensation=False, normalize=True, f_start=1.3e9, f_stop=3.7e9, voxel=[.45, .4, .35]),
|
||||
dict(compensation=False, normalize=False, speed=0., pitch=0., radius=0),
|
||||
dict(compensation=True, normalize=True, subtract_mean=True, mean_count=1),
|
||||
dict(compensation=True, normalize=True, x_size=3., z_size=2., y_min=-.2, y_max=1.4),
|
||||
# CLEAN stops below its thresholds; the background gate hides the residual.
|
||||
point_scene(REFLECTORS, noise=.02, seed=1),
|
||||
# CLEAN stops at SAR_CLEAN_MAX_STEPS; without the gate the residual stays visible.
|
||||
point_scene(REFLECTORS, noise=.01, seed=3, bg_gate=False),
|
||||
# The stop at the background of each range (SAR_CLEAN_STOP_RANGE_BG) adds two steps to
|
||||
# those of the stop at the background of the whole volume.
|
||||
point_scene([(.2, 0., 2., 1.), (-.5, .1, 4.4, .08)], noise=.01, seed=4, max_depth=5.),
|
||||
# A narrowed frame: CLEAN still searches the whole scene, so the reflector at X = -0.8 m
|
||||
# just outside the frame is subtracted instead of its sidelobe becoming an object.
|
||||
point_scene(REFLECTORS, noise=.02, seed=19, x_size=1.6, z_size=.8),
|
||||
]
|
||||
arrays, configurations = {}, []
|
||||
for index, settings in enumerate(cases):
|
||||
request = scene()
|
||||
request.update(settings)
|
||||
# Match the float32 live settings of the native fixture.
|
||||
for key in ("min_depth", "max_depth", "speed", "pitch", "ground", "power",
|
||||
"x_size", "z_size", "y_min", "y_max"):
|
||||
if key in request:
|
||||
request[key] = float(np.float32(request[key]))
|
||||
request["voxel"] = [float(np.float32(step)) for step in request["voxel"]]
|
||||
if index == 1:
|
||||
# A separate, unequal capture window per output instead of interleaving.
|
||||
for member in [*request["previous"], request["latest"]]:
|
||||
for trace in member["traces"]:
|
||||
if trace["output"] == 0:
|
||||
trace["end"] = trace["start"] + 80_000_000
|
||||
else:
|
||||
trace["start"] += 95_000_000
|
||||
with tempfile.TemporaryDirectory(prefix="sar3d_notebook_") as directory:
|
||||
expected, hashes = reference(notebook, request, Path(directory))
|
||||
if index == 0:
|
||||
arrays["request"] = np.array(json.dumps(request, separators=(",", ":")))
|
||||
configurations.append(dict(
|
||||
settings={key: request[key] for key in settings},
|
||||
windows=[[[trace["start"], trace["end"]] for trace in member["traces"]]
|
||||
for member in [*request["previous"], request["latest"]]],
|
||||
))
|
||||
arrays.update({f"case_{index}_{key}": value for key, value in expected.items()})
|
||||
arrays["cases"] = np.array(json.dumps(configurations, separators=(",", ":")))
|
||||
marks_request, arrays["marks"] = marks_reference(notebook)
|
||||
arrays["marks_request"] = np.array(json.dumps(marks_request))
|
||||
arrays["provenance"] = np.array(json.dumps(dict(
|
||||
notebook=notebook.name, cell_source_sha256=hashes, cells=[1, 5], cases=len(cases),
|
||||
numpy=np.__version__, scipy=scipy.__version__,
|
||||
method="Unchanged notebook AST functions; per-frame NumPy previous-mean subtraction; "
|
||||
"max projections of the sar_prepare_display volume, its background, CLEAN objects and marks",
|
||||
), sort_keys=True))
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(output, **arrays)
|
||||
print(f"Wrote {output}: {output.stat().st_size} bytes, {len(cases)} notebook references")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--notebook", type=Path, default=ROOT / "Multi_depthnorm_clean.ipynb")
|
||||
parser.add_argument("--output", type=Path, default=REFERENCE)
|
||||
parser.add_argument("--gpr-script", type=Path, default=ROOT / "Horns_motion_4x4_ground.py")
|
||||
parser.add_argument("--gpr-output", type=Path, default=REFERENCE.with_name("gpr_reference.npz"))
|
||||
args = parser.parse_args()
|
||||
generate(args.notebook, args.output)
|
||||
generate_gpr(args.notebook, args.gpr_script, args.gpr_output)
|
||||
Binary file not shown.
@@ -0,0 +1,575 @@
|
||||
#include "channel_bundle_support.hpp"
|
||||
#include "data_preprocessor.hpp"
|
||||
#include "locator/tcp_server.hpp"
|
||||
#include "object_approach_filter.hpp"
|
||||
#include "processing_live_config.hpp"
|
||||
#include "runtime_wait.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
#include "tty_protocol_writer.h"
|
||||
#include "switch_sequencer.h"
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/wait.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <exception>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <random>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace {
|
||||
using namespace radar;
|
||||
using Json = nlohmann::json;
|
||||
|
||||
void require(bool condition, const char* message) {
|
||||
if (!condition) {
|
||||
throw std::runtime_error(message);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Callable>
|
||||
void expect_error(Callable operation) {
|
||||
try {
|
||||
operation();
|
||||
} catch (const std::bad_alloc&) {
|
||||
throw; // A malformed count must be rejected before allocating.
|
||||
} catch (const std::exception&) {
|
||||
return;
|
||||
}
|
||||
throw std::runtime_error("Expected operation to reject malformed input");
|
||||
}
|
||||
|
||||
struct Workspace {
|
||||
std::filesystem::path path;
|
||||
Workspace() {
|
||||
char pattern[] = "/tmp/radar-native-XXXXXX";
|
||||
const auto* created = ::mkdtemp(pattern);
|
||||
require(created != nullptr, "mkdtemp failed");
|
||||
path = created;
|
||||
}
|
||||
~Workspace() { std::filesystem::remove_all(path); }
|
||||
};
|
||||
|
||||
struct RingName {
|
||||
std::string name;
|
||||
explicit RingName(const std::string& suffix = {})
|
||||
: name("/radar_native_test_" + std::to_string(::getpid()) + suffix) {
|
||||
::shm_unlink(name.c_str());
|
||||
}
|
||||
~RingName() { ::shm_unlink(name.c_str()); }
|
||||
};
|
||||
|
||||
auto save_bundle(const Workspace& workspace, const char* name, const ipc::RawSweepCollection& collection)
|
||||
-> std::string {
|
||||
const auto path = workspace.path / name;
|
||||
const auto bytes = ipc::serialize_raw_collection(collection);
|
||||
std::ofstream(path, std::ios::binary).write(
|
||||
reinterpret_cast<const char*>(bytes.data()), static_cast<std::streamsize>(bytes.size()));
|
||||
return path.string();
|
||||
}
|
||||
|
||||
void write_u32(std::vector<std::uint8_t>& bytes, std::size_t offset, std::uint32_t value) {
|
||||
require(offset + sizeof(value) <= bytes.size(), "invalid test offset");
|
||||
std::memcpy(bytes.data() + offset, &value, sizeof(value));
|
||||
}
|
||||
|
||||
void test_wire_format() {
|
||||
ipc::RawSweepCollection raw{.collection_id = 7, .monotonic_ns = 42,
|
||||
.capture_start_ns = 21, .capture_end_ns = 40,
|
||||
.traces = {{.combo = {1, 2}, .frequency_hz = {1, 2},
|
||||
.s21 = {{5, 6}, {7, 8}},
|
||||
.capture_start_ns = 25, .capture_end_ns = 39}}};
|
||||
ipc::ResultCollection results{.collection_id = 7, .monotonic_ns = 0x0102030405060708ULL,
|
||||
.processing_duration_ns = 17, .settings_revision = 0xfedcba9876543210ULL,
|
||||
.capture_start_ns = 21, .capture_end_ns = 40, .speed_m_s = -1.5F,
|
||||
.collection_payloads = {
|
||||
{.kind = ipc::ResultKind::TraceComplex, .frequency_hz = {1}, .trace = {{2, 3}}},
|
||||
{.kind = ipc::ResultKind::ScalarF32, .scalar_value = 4},
|
||||
{.kind = ipc::ResultKind::ImageF32, .image_x_axis = {1, 2},
|
||||
.image_y_axis = {3}, .image_values = {4, 5}},
|
||||
{.kind = ipc::ResultKind::TableF32, .table_columns = 2, .table_values = {6, 7}},
|
||||
{.kind = ipc::ResultKind::ImageF32, .image_y_axis = {1}},
|
||||
{.kind = ipc::ResultKind::TableF32, .table_columns = 3}},
|
||||
.blocks = {{.combo = {1, 2}, .payloads = {{.kind = ipc::ResultKind::ScalarF32}}}}};
|
||||
const auto raw_bytes = ipc::serialize_raw_collection(raw);
|
||||
const auto pre_bytes = ipc::serialize_preprocessed_collection(raw);
|
||||
const auto result_bytes = ipc::serialize_result_collection(results);
|
||||
const std::array<std::uint8_t, 64> result_header{
|
||||
'R', 'S', 'L', '3', 7, 0, 0, 0, 0, 0, 0, 0,
|
||||
8, 7, 6, 5, 4, 3, 2, 1, 17, 0, 0, 0, 0, 0, 0, 0,
|
||||
0x10, 0x32, 0x54, 0x76, 0x98, 0xba, 0xdc, 0xfe,
|
||||
21, 0, 0, 0, 0, 0, 0, 0, 40, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0xc0, 0xbf, 6, 0, 0, 0, 1, 0, 0, 0,
|
||||
};
|
||||
require(std::equal(result_header.begin(), result_header.end(), result_bytes.begin()),
|
||||
"RSL3 must preserve exact acquisition identity and settings in its 64-byte header");
|
||||
require(std::memcmp(raw_bytes.data(), "RAW3", 4) == 0 &&
|
||||
std::memcmp(pre_bytes.data(), "PRP3", 4) == 0,
|
||||
"trace collections must use the S21-only version 3 format");
|
||||
// Collection header + one trace header + two 12-byte points + capture trailer.
|
||||
require(raw_bytes.size() == 24 + 12 + 2 * 12 + 16 + 4 + 16 && pre_bytes.size() == raw_bytes.size(),
|
||||
"trace wire format must contain only frequency and S21 samples");
|
||||
auto old_raw = raw_bytes;
|
||||
auto old_pre = pre_bytes;
|
||||
write_u32(old_raw, 0, 0x32574152U);
|
||||
write_u32(old_pre, 0, 0x32525050U);
|
||||
expect_error([&] { (void)ipc::deserialize_raw_collection(old_raw); });
|
||||
expect_error([&] { (void)ipc::deserialize_preprocessed_collection(old_pre); });
|
||||
require(ipc::serialize_raw_collection(ipc::deserialize_raw_collection(raw_bytes)) == raw_bytes,
|
||||
"raw roundtrip changed data");
|
||||
require(ipc::serialize_preprocessed_collection(ipc::deserialize_preprocessed_collection(pre_bytes)) == pre_bytes,
|
||||
"preprocessed roundtrip changed data");
|
||||
for (const std::size_t tail : {36U, 20U}) {
|
||||
expect_error([&] { (void)ipc::deserialize_raw_collection(std::span(raw_bytes).first(raw_bytes.size() - tail)); });
|
||||
expect_error([&] { (void)ipc::deserialize_preprocessed_collection(std::span(pre_bytes).first(pre_bytes.size() - tail)); });
|
||||
}
|
||||
// Deterministic random mutations include wire-compatible valid frames as seeds.
|
||||
std::mt19937 random(0x52414452U);
|
||||
for (unsigned iteration = 0; iteration < 10'000U; ++iteration) {
|
||||
auto bytes = iteration % 2U == 0U ? raw_bytes : pre_bytes;
|
||||
for (unsigned edit = 0; edit < 1U + random() % 5U; ++edit) {
|
||||
bytes[random() % bytes.size()] = static_cast<std::uint8_t>(random());
|
||||
}
|
||||
if (iteration % 4U == 0U) {
|
||||
bytes.resize(random() % bytes.size());
|
||||
}
|
||||
try {
|
||||
if (iteration % 2U == 0U) { (void)ipc::deserialize_raw_collection(bytes); }
|
||||
else { (void)ipc::deserialize_preprocessed_collection(bytes); }
|
||||
} catch (const std::bad_alloc&) { throw; }
|
||||
catch (const std::runtime_error&) {}
|
||||
}
|
||||
}
|
||||
|
||||
void test_configuration(const std::string& seed_path, const Workspace& workspace) {
|
||||
std::ifstream input(seed_path);
|
||||
const auto seed = Json::parse(input);
|
||||
const auto config_path = workspace.path / "config.json";
|
||||
const auto save = [&](const Json& config) { std::ofstream(config_path) << config; };
|
||||
save(seed);
|
||||
const auto config = config::load_run_config(config_path.string());
|
||||
auto no_taps = seed;
|
||||
no_taps["rings"]["preprocessed_tap"]["name"] = "";
|
||||
save(no_taps);
|
||||
const auto disabled = config::load_run_config(config_path.string()).rings;
|
||||
require(disabled.preprocessed_tap.name.empty(),
|
||||
"explicit empty tap names must disable the optional rings");
|
||||
for (const auto* key : {"raw", "preprocessed", "results"}) {
|
||||
auto invalid = no_taps;
|
||||
invalid["rings"][key]["name"] = "";
|
||||
save(invalid);
|
||||
expect_error([&] { (void)config::load_run_config(config_path.string()); });
|
||||
}
|
||||
for (const auto* port : {"port1", "port2"}) {
|
||||
auto with_driver = seed;
|
||||
with_driver["switches"][port]["driver"] = "h7992";
|
||||
save(with_driver);
|
||||
(void)config::load_run_config(config_path.string());
|
||||
for (const auto& driver : {Json("hmc349a"), Json("unknown"), Json(7)}) {
|
||||
with_driver["switches"][port]["driver"] = driver;
|
||||
save(with_driver);
|
||||
expect_error([&] { (void)config::load_run_config(config_path.string()); });
|
||||
}
|
||||
}
|
||||
auto with_object_calibration = seed;
|
||||
with_object_calibration["preprocess"]["s21"]["object_calibration"] =
|
||||
Json{{"set_name", "reflector"}, {"bundle_path", "corrections.bin"}};
|
||||
save(with_object_calibration);
|
||||
const auto selected = config::load_run_config(config_path.string()).preprocess.s21.object_calibration;
|
||||
require(selected.bundle_path == "corrections.bin",
|
||||
"object calibration asset did not load");
|
||||
with_object_calibration["preprocess"]["s21"].erase("object_calibration");
|
||||
save(with_object_calibration);
|
||||
require(config::load_run_config(config_path.string()).preprocess.s21.object_calibration.bundle_path.empty(),
|
||||
"absent object calibration was not disabled");
|
||||
for (const auto& invalid_asset : {Json(nullptr), Json{{"bundle_path", true}}}) {
|
||||
with_object_calibration["preprocess"]["s21"]["object_calibration"] = invalid_asset;
|
||||
save(with_object_calibration);
|
||||
expect_error([&] { (void)config::load_run_config(config_path.string()); });
|
||||
}
|
||||
for (const auto& [pointer, value] : std::array<std::pair<const char*, double>, 3>{{
|
||||
{"/run/combos/0/input", 0.0000001},
|
||||
{"/rings/raw/slot_size_bytes", 65}, {"/gpr/rx_geometry/0/x_m", 1e100}}}) {
|
||||
auto invalid = seed;
|
||||
invalid[Json::json_pointer(pointer)] = value;
|
||||
save(invalid);
|
||||
expect_error([&] { (void)config::load_run_config(config_path.string()); });
|
||||
}
|
||||
const auto live_path = workspace.path / "live.json";
|
||||
std::ofstream(live_path) << Json{{"bscan_gain", 2}, {"settings_revision", 11},
|
||||
{"sar3d_bg_gate_enabled", false}, {"sar3d_clean_enabled", false},
|
||||
{"legacy_gpr_mode", false}, {"relative_permittivity", "obsolete"}};
|
||||
processing::ProcessingLiveConfigLoader loader(live_path.string(), config);
|
||||
const auto loaded = loader.refresh_if_needed();
|
||||
require(loaded.bscan_gain == 2 && !loaded.sar3d_bg_gate_enabled && !loaded.sar3d_clean_enabled,
|
||||
"valid live config did not load");
|
||||
const auto revision = loader.revision();
|
||||
for (const auto& invalid : {Json{{"bscan_gain", 1e100}},
|
||||
Json{{"gpr_input_positions", {0.0000001}}}, Json{{"bscan_gain", true}},
|
||||
Json{{"processor_mode", "legacy_gpr"}}}) {
|
||||
std::ofstream(live_path) << invalid;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(110));
|
||||
const auto current = loader.refresh_if_needed();
|
||||
require(current.bscan_gain == 2 && loader.revision() == revision,
|
||||
"invalid live config replaced the last valid settings");
|
||||
}
|
||||
}
|
||||
|
||||
void test_ring() {
|
||||
RingName name;
|
||||
expect_error([&] { (void)ipc::ShmRing::open_or_create(name.name, 2, 65); });
|
||||
expect_error([&] { (void)ipc::ShmRing::open_or_create(name.name, 0xffffffffU, 0xfffffff8U); });
|
||||
auto ring = ipc::ShmRing::open_or_create(name.name, 2, 4096);
|
||||
const auto child = ::fork();
|
||||
require(child >= 0, "fork failed");
|
||||
if (child == 0) {
|
||||
std::array<std::uint64_t, 512> words{};
|
||||
for (std::uint64_t sequence = 1; sequence <= 20'000U; ++sequence) {
|
||||
words.fill(sequence);
|
||||
if (!ring.push({reinterpret_cast<const std::uint8_t*>(words.data()), sizeof(words)})) {
|
||||
::_exit(2);
|
||||
}
|
||||
}
|
||||
::_exit(0);
|
||||
}
|
||||
std::uint64_t previous = 0;
|
||||
std::vector<std::uint8_t> payload;
|
||||
int status = 0;
|
||||
bool finished = false;
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10);
|
||||
try {
|
||||
while (true) {
|
||||
if (ring.pop(payload)) {
|
||||
require(payload.size() == 4096, "stress payload changed size");
|
||||
std::uint64_t sequence = 0;
|
||||
std::memcpy(&sequence, payload.data(), sizeof(sequence));
|
||||
require(sequence > previous, "stress ring yielded duplicate or older data");
|
||||
for (std::size_t offset = 0; offset < payload.size(); offset += sizeof(sequence)) {
|
||||
std::uint64_t word = 0;
|
||||
std::memcpy(&word, payload.data() + offset, sizeof(word));
|
||||
require(word == sequence, "stress ring accepted a torn payload");
|
||||
}
|
||||
previous = sequence;
|
||||
} else if (finished) { break; }
|
||||
finished = finished || ::waitpid(child, &status, WNOHANG) == child;
|
||||
require(std::chrono::steady_clock::now() < deadline, "ring stress timed out");
|
||||
}
|
||||
require(WIFEXITED(status) && WEXITSTATUS(status) == 0 && previous != 0,
|
||||
"ring stress writer failed");
|
||||
} catch (...) {
|
||||
::kill(child, SIGKILL);
|
||||
if (!finished) { (void)::waitpid(child, &status, 0); }
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void test_acquisition_identity() {
|
||||
processing::ObjectApproachFilter filter;
|
||||
const std::vector<processing::ObjectApproachFilter::Point> points{{0, 2}};
|
||||
require(!filter.confirm(points, 1, 1'000'000'000ULL, 0, 0, 2).front(), "first frame confirmed a track");
|
||||
require(filter.confirm(points, 1, 2'000'000'000ULL, 0, 0, 2).front(),
|
||||
"reused collection id replaced a distinct acquisition");
|
||||
require(filter.confirm(points, 1, 2'000'000'000ULL, 0, 0, 2).front(),
|
||||
"reprocessing duplicated a frame or lost its predecessor");
|
||||
require(!filter.confirm(points, 7, 1'000'000'000ULL, 0, 0, 2).front(),
|
||||
"an earlier capture time failed to reset replay history");
|
||||
}
|
||||
|
||||
void test_interruptible_wait() {
|
||||
std::atomic<bool> stop{false};
|
||||
std::jthread request_stop([&] {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
stop.store(true);
|
||||
});
|
||||
const auto start = std::chrono::steady_clock::now();
|
||||
require(config::wait_or_stop(0xffffffffU, stop), "large wait ignored stop");
|
||||
require(std::chrono::steady_clock::now() - start < std::chrono::seconds(1),
|
||||
"large wait delayed shutdown");
|
||||
}
|
||||
|
||||
void test_cosine_notch_taper() {
|
||||
// Band 2.0-2.1 GHz with 0.1 GHz shoulders: 1 at the outer edges, 0.5 halfway,
|
||||
// 0 from the band edges inwards.
|
||||
const std::vector<float> frequency_hz{1.85e9F, 1.9e9F, 1.95e9F, 2.0e9F, 2.05e9F,
|
||||
2.1e9F, 2.15e9F, 2.2e9F, 2.25e9F};
|
||||
const std::vector<float> expected{1.0F, 1.0F, 0.5F, 0.0F, 0.0F, 0.0F, 0.5F, 1.0F, 1.0F};
|
||||
const auto mask = preprocessing::build_notch_mask(frequency_hz,
|
||||
{.enabled = true, .bands_hz = {{2.0e9F, 2.1e9F}}, .taper_width_hz = 1e8F, .taper_type = "cosine"});
|
||||
for (std::size_t index = 0U; index < expected.size(); ++index) {
|
||||
require(std::abs(mask[index] - expected[index]) < 1e-3F, "cosine notch shoulders are not 1 -> 0 towards the band");
|
||||
}
|
||||
}
|
||||
|
||||
void test_asset_reload_preserves_data(const Workspace& workspace) {
|
||||
const auto save = [&](const char* name, float sample) {
|
||||
return save_bundle(workspace, name, {.traces = {
|
||||
{.frequency_hz = {1}, .s21 = {{sample, 0}}}}});
|
||||
};
|
||||
const auto open = save("open.bin", 1);
|
||||
const auto nan = save("nan.bin", std::numeric_limits<float>::quiet_NaN());
|
||||
preprocessing::S21CalibrationBundle s21_calibration;
|
||||
s21_calibration.load(save("through.bin", 2));
|
||||
expect_error([&] { s21_calibration.load(nan); });
|
||||
const std::array<float, 1> frequency{1};
|
||||
std::array<ipc::Complex32, 1> samples{{{6, 8}}};
|
||||
s21_calibration.apply({}, frequency, samples);
|
||||
require(samples[0].re == 3 && samples[0].im == 4, "failed S21 reload changed previous calibration");
|
||||
const std::array<float, 1> nan_axis{std::numeric_limits<float>::quiet_NaN()};
|
||||
expect_error([&] { s21_calibration.apply({}, nan_axis, samples); });
|
||||
preprocessing::S21ReferenceBundle reference;
|
||||
reference.load(open, s21_calibration, {{}});
|
||||
expect_error([&] { reference.load(nan, s21_calibration, {{}}); });
|
||||
ipc::SweepTraceBlock trace{.frequency_hz = {1}, .s21 = {{2, 0}}};
|
||||
reference.apply(trace);
|
||||
require(trace.s21[0].re == 1.5F, "failed reference reload changed the calibrated reference");
|
||||
}
|
||||
|
||||
void test_object_calibration(const Workspace& workspace) {
|
||||
const ipc::SweepTraceBlock first{.combo = {7, 3}, .frequency_hz = {1e9F, 2e9F},
|
||||
.s21 = {{2, 3}, {4, 5}}, .capture_start_ns = 5, .capture_end_ns = 15};
|
||||
const ipc::SweepTraceBlock second{.combo = {1, 9}, .frequency_hz = {3e9F, 4e9F, 5e9F},
|
||||
.s21 = {{6, 7}, {8, 9}, {10, 11}}};
|
||||
auto factors_first = first;
|
||||
factors_first.s21 = {{0, 2}, {2, 0}};
|
||||
auto factors_second = second;
|
||||
factors_second.s21 = {{1, -1}, {-1, 0}, {0, 1}};
|
||||
preprocessing::S21ObjectCalibrationBundle correction;
|
||||
auto trace = first;
|
||||
correction.apply(trace);
|
||||
require(ipc::serialize_raw_collection({.traces = {trace}}) == ipc::serialize_raw_collection({.traces = {first}}),
|
||||
"disabled object calibration changed a trace");
|
||||
correction.load(save_bundle(workspace, "object.bin", {.traces = {factors_second, factors_first}}));
|
||||
correction.validate_combos({first.combo, second.combo});
|
||||
expect_error([&] { correction.validate_combos({{0, 0}}); });
|
||||
for (auto invalid : {first, second}) {
|
||||
invalid.combo = {0, 0};
|
||||
expect_error([&] { correction.apply(invalid); });
|
||||
}
|
||||
auto mismatch = second;
|
||||
mismatch.frequency_hz[1] += 1e8F; // Same endpoints and count; interior grid differs.
|
||||
expect_error([&] { correction.apply(mismatch); });
|
||||
mismatch = second;
|
||||
mismatch.s21.pop_back();
|
||||
expect_error([&] { correction.apply(mismatch); });
|
||||
auto nonfinite = factors_first;
|
||||
nonfinite.s21[0].re = std::numeric_limits<float>::quiet_NaN();
|
||||
expect_error([&] { correction.load(save_bundle(workspace, "object_nan.bin", {.traces = {nonfinite}})); });
|
||||
expect_error([&] { correction.load(save_bundle(workspace, "object_duplicate.bin", {.traces = {factors_first, factors_first}})); });
|
||||
auto expected = first;
|
||||
expected.s21 = {{-6, 4}, {8, 10}};
|
||||
trace = first;
|
||||
correction.apply(trace);
|
||||
require(ipc::serialize_raw_collection({.traces = {trace}}) == ipc::serialize_raw_collection({.traces = {expected}}),
|
||||
"object calibration changed timing, pair identity, or multiplication after a failed reload");
|
||||
trace = second;
|
||||
correction.apply(trace);
|
||||
require(trace.s21[0].re == 13 && trace.s21[0].im == 1 && trace.s21[1].re == -8 && trace.s21[1].im == -9 &&
|
||||
trace.s21[2].re == -11 && trace.s21[2].im == 10, "object calibration mixed pairs or frequency grids");
|
||||
trace = first;
|
||||
trace.s21[0] = {std::numeric_limits<float>::max(), std::numeric_limits<float>::max()};
|
||||
expect_error([&] { correction.apply(trace); });
|
||||
correction.load("");
|
||||
trace = first;
|
||||
correction.apply(trace);
|
||||
require(trace.s21[0].re == 2 && trace.s21[0].im == 3, "clearing object calibration left stale factors");
|
||||
}
|
||||
|
||||
void test_object_calibration_pipeline(const Workspace& workspace) {
|
||||
const ipc::SweepTraceBlock trace{.combo = {7, 3}, .frequency_hz = {1e9F, 2e9F},
|
||||
.s21 = {{2, 4}, {6, 8}}, .capture_start_ns = 5, .capture_end_ns = 15};
|
||||
auto standard = trace;
|
||||
standard.s21 = {{2, 0}, {2, 0}};
|
||||
auto reference = trace;
|
||||
reference.s21 = {{1, 2}, {2, 2}};
|
||||
auto factors = trace;
|
||||
factors.s21 = {{0, 2}, {5, 1}};
|
||||
config::RunConfig config;
|
||||
config.preprocess.s21.calibration.bundle_path =
|
||||
save_bundle(workspace, "pipeline_through.bin", {.traces = {standard}});
|
||||
config.preprocess.s21.reference.bundle_path =
|
||||
save_bundle(workspace, "pipeline_reference.bin", {.traces = {reference}});
|
||||
config.run_combos = {trace.combo};
|
||||
config.preprocess.s21.object_calibration.bundle_path =
|
||||
save_bundle(workspace, "pipeline_object.bin", {.traces = {factors}});
|
||||
config.preprocess.notch = {.enabled = true, .bands_hz = {{2e9F, 2e9F}}, .taper_type = "hard"};
|
||||
RingName raw_name("_object_raw"), pre_name("_object_pre");
|
||||
auto raw_ring = ipc::ShmRing::open_or_create(raw_name.name, 2, 4096);
|
||||
auto pre_ring = ipc::ShmRing::open_or_create(pre_name.name, 2, 4096);
|
||||
preprocessing::DataPreprocessor preprocessor(config, raw_ring, pre_ring);
|
||||
require(raw_ring.push(ipc::serialize_raw_collection({.collection_id = 41, .capture_start_ns = 5,
|
||||
.capture_end_ns = 15, .traces = {trace}})), "test raw ring could not hold a sweep");
|
||||
std::atomic<bool> stop{false};
|
||||
std::exception_ptr error;
|
||||
std::jthread worker([&] { try { preprocessor.run(stop); } catch (...) { error = std::current_exception(); } });
|
||||
std::vector<std::uint8_t> bytes;
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2);
|
||||
while (bytes.empty() && std::chrono::steady_clock::now() < deadline) {
|
||||
(void)pre_ring.pop(bytes);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
stop.store(true);
|
||||
worker.join();
|
||||
if (error) { std::rethrow_exception(error); }
|
||||
require(!bytes.empty(), "object-corrected preprocessed collection was not published");
|
||||
const auto result = ipc::deserialize_preprocessed_collection(bytes);
|
||||
const auto& output = result.traces.at(0);
|
||||
require(result.collection_id == 41 && result.capture_start_ns == 5 && result.capture_end_ns == 15 &&
|
||||
output.combo == trace.combo && output.capture_start_ns == 5 && output.capture_end_ns == 15,
|
||||
"object calibration lost capture identity or timing");
|
||||
require(output.s21[0].re == -2 && output.s21[0].im == 1 && output.s21[1].re == 0 && output.s21[1].im == 0,
|
||||
"object calibration ran before existing preprocessing or was applied more than once");
|
||||
|
||||
}
|
||||
|
||||
void test_locator_packet_errors() {
|
||||
int sockets[2]{};
|
||||
require(::socketpair(AF_UNIX, SOCK_STREAM, 0, sockets) == 0, "socketpair failed");
|
||||
std::atomic<double> speed{0};
|
||||
std::atomic<std::int64_t> at_ns{0};
|
||||
locator::ClientSession client(sockets[0], "regression", 2, 1024);
|
||||
client.start(speed, at_ns);
|
||||
const auto send_json = [&](const std::string& json) {
|
||||
std::vector<std::uint8_t> bytes(8 + json.size());
|
||||
write_u32(bytes, 4, static_cast<std::uint32_t>(json.size()));
|
||||
std::memcpy(bytes.data() + 8, json.data(), json.size());
|
||||
const auto sent = ::send(sockets[1], bytes.data(), bytes.size(), MSG_NOSIGNAL);
|
||||
if (sent != static_cast<ssize_t>(bytes.size())) {
|
||||
throw std::runtime_error(std::string("send test packet failed: ") + std::strerror(errno));
|
||||
}
|
||||
};
|
||||
send_json("{\"vlc\":1e400}");
|
||||
send_json("{\"vlc\":1e308}");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
require(at_ns.load() == 0 && speed.load() == 0, "invalid socket speed reached the processor");
|
||||
send_json("{\"vlc\":3.5}");
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2);
|
||||
while (at_ns.load() == 0 && std::chrono::steady_clock::now() < deadline) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
require(speed.load() == 3.5 && at_ns.load() > 0, "locator stopped after oversized JSON number");
|
||||
client.request_stop();
|
||||
client.join();
|
||||
::close(sockets[1]);
|
||||
require(client.has_exited(), "locator session did not finish both loops");
|
||||
locator::LocatorServerConfig config{.host = "127.0.0.1", .port = 0};
|
||||
locator::TcpServer server(config);
|
||||
for (unsigned iteration = 0; iteration < 30; ++iteration) { server.start(); server.stop(); }
|
||||
}
|
||||
|
||||
void test_tty_sweep_overflow(const Workspace& workspace) {
|
||||
TtyProtocolWriter writer((workspace.path / "overflow_tty").string(), 32'096);
|
||||
const int fd = ::open(writer.path().c_str(), O_RDONLY | O_NONBLOCK);
|
||||
require(fd >= 0, "Cannot open test PTY");
|
||||
std::vector<std::uint16_t> words(8'000, 42);
|
||||
for (std::uint64_t id = 1; id <= 100; ++id) {
|
||||
writer.enqueue_sweep(words, {id, id, id * 100, id * 100 + 50, 1, 2, false});
|
||||
}
|
||||
require(writer.stats().ring_overflows > 0, "test did not overflow the queue");
|
||||
std::vector<std::uint8_t> received;
|
||||
std::uint64_t last_id = 0;
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2);
|
||||
while (last_id < 100 && std::chrono::steady_clock::now() < deadline) {
|
||||
std::array<std::uint8_t, 8192> chunk{};
|
||||
const auto n = ::read(fd, chunk.data(), chunk.size());
|
||||
if (n > 0) { received.insert(received.end(), chunk.begin(), chunk.begin() + n); }
|
||||
while (received.size() >= 16'048) {
|
||||
require(std::memcmp(received.data(), "RPH1", 4) == 0, "overflow truncated a sweep header");
|
||||
const auto value = [&](std::size_t at, unsigned size) {
|
||||
std::uint64_t result = 0;
|
||||
for (unsigned i = 0; i < size; ++i) { result |= std::uint64_t{received[at + i]} << (8U * i); }
|
||||
return result;
|
||||
};
|
||||
require(value(4, 4) == 16'000 && value(8, 8) > last_id, "lost framing or source order");
|
||||
last_id = value(8, 8);
|
||||
require(value(16, 8) == last_id && value(24, 8) == last_id * 100 &&
|
||||
value(32, 8) == last_id * 100 + 50 && value(40, 2) == 1 && value(42, 2) == 2,
|
||||
"sweep lost source identity, timing, or combo");
|
||||
require(received[48] == 42 && received[16'047] == 0, "corrupt point payload");
|
||||
received.erase(received.begin(), received.begin() + 16'048);
|
||||
}
|
||||
if (n <= 0) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); }
|
||||
}
|
||||
::close(fd);
|
||||
require(last_id == 100 && received.empty(), "latest complete sweep did not arrive");
|
||||
}
|
||||
|
||||
void test_switch_backlog() {
|
||||
drivers::H7992MinimalDriverSettings settings{.mode = config::DriverMode::Mock};
|
||||
rph::SwitchSequencer sequencer({.combos = {{0, 0}, {0, 1}}},
|
||||
std::make_unique<drivers::H7992MinimalDriver>(settings),
|
||||
std::make_unique<drivers::H7992MinimalDriver>(settings));
|
||||
sequencer.open();
|
||||
sequencer.session_started();
|
||||
require(!sequencer.on_sweep_end(false), "duration limit requested a new session");
|
||||
require(sequencer.current_combo().output_pos == 0 && sequencer.cycle_id() == 1,
|
||||
"duration limit switched hardware before the physical sweep ended");
|
||||
require(sequencer.on_sweep_end(), "GPIO change must request a new source session");
|
||||
for (unsigned i = 0; i < 10; ++i) {
|
||||
require(sequencer.current_dirty(), "queued pre-switch sweep incorrectly clean");
|
||||
require(!sequencer.on_sweep_end(), "dirty backlog requested another session");
|
||||
require(sequencer.current_combo().output_pos == 1 && sequencer.cycle_id() == 1,
|
||||
"dirty backlog advanced the combo or cycle");
|
||||
}
|
||||
sequencer.session_started();
|
||||
require(!sequencer.current_dirty(), "new source session must permit a complete sweep");
|
||||
sequencer.on_sweep_end();
|
||||
require(sequencer.current_combo().output_pos == 0 && sequencer.cycle_id() == 2,
|
||||
"clean cycle did not advance");
|
||||
rph::SwitchSequencer single({.combos = {{0, 0}}},
|
||||
std::make_unique<drivers::H7992MinimalDriver>(settings),
|
||||
std::make_unique<drivers::H7992MinimalDriver>(settings));
|
||||
single.open(); single.session_started();
|
||||
require(!single.on_sweep_end() && single.cycle_id() == 2 && !single.current_dirty(),
|
||||
"unchanged combination restarted its source or stopped advancing cycles");
|
||||
}
|
||||
|
||||
void test_unread_tty_shutdown(const Workspace& workspace) {
|
||||
TtyProtocolWriter writer((workspace.path / "tty").string(), 160'048);
|
||||
const std::vector<std::uint16_t> words(80'000, 42);
|
||||
writer.enqueue_sweep(words, {1, 1, 100, 200});
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
const auto start = std::chrono::steady_clock::now();
|
||||
writer.shutdown();
|
||||
require(std::chrono::steady_clock::now() - start < std::chrono::seconds(1),
|
||||
"unread tty blocked shutdown");
|
||||
writer.shutdown();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
try {
|
||||
require(argc == 2, "Pass the simulator config fixture path");
|
||||
Workspace workspace;
|
||||
test_wire_format();
|
||||
test_configuration(argv[1], workspace);
|
||||
test_asset_reload_preserves_data(workspace);
|
||||
test_object_calibration(workspace);
|
||||
test_object_calibration_pipeline(workspace);
|
||||
test_ring();
|
||||
test_acquisition_identity();
|
||||
test_interruptible_wait();
|
||||
test_cosine_notch_taper();
|
||||
test_tty_sweep_overflow(workspace);
|
||||
test_switch_backlog();
|
||||
test_unread_tty_shutdown(workspace);
|
||||
test_locator_packet_errors();
|
||||
std::cout << "native regressions passed (10,000 decoder mutations; 20,000 concurrent ring frames)\n";
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << "native regression failed: " << error.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
// SDK contract fixture: Stop retains queued input; only a new descriptor is fresh.
|
||||
#include "x502api.h"
|
||||
#include "e502api.h"
|
||||
#include <algorithm>
|
||||
#include <csignal>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace {
|
||||
struct Session { unsigned id; bool consumed = false; std::vector<uint32_t> input; };
|
||||
unsigned sessions = 0;
|
||||
void record(const char* name) { std::ofstream(std::getenv("RPH_SDK_LOG"), std::ios::app) << name << '\n'; }
|
||||
Session& session(t_x502_hnd hnd) { return *reinterpret_cast<Session*>(hnd); }
|
||||
void generate(Session& s) {
|
||||
for (unsigned sweep = 0; sweep < 4; ++sweep) {
|
||||
for (unsigned step = 0; step < 2; ++step) {
|
||||
for (unsigned n = 0; n < 8; ++n) {
|
||||
const bool high = n % 4 >= 2;
|
||||
s.input.push_back(high ? 0 : 10000 * s.id);
|
||||
s.input.push_back((1U << 13) | step | (high ? 2 : 0));
|
||||
}
|
||||
}
|
||||
for (unsigned n = 0; n < 4; ++n) { s.input.push_back(0); s.input.push_back(0); }
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
extern "C" int32_t E502_OpenUsb(t_x502_hnd hnd, const char *serial) { record("Open"); return 0; }
|
||||
extern "C" int32_t X502_AsyncOutDig(t_x502_hnd hnd, uint32_t val, uint32_t msk) { return 0; }
|
||||
extern "C" int32_t X502_Close(t_x502_hnd hnd) { record("Close"); return 0; }
|
||||
extern "C" int32_t X502_Configure(t_x502_hnd hnd, uint32_t flags) { record("Configure"); return 0; }
|
||||
extern "C" t_x502_hnd X502_Create(void) { record("Create"); auto* s = new Session{++sessions}; generate(*s); return reinterpret_cast<t_x502_hnd>(s); }
|
||||
extern "C" int32_t X502_Free(t_x502_hnd hnd) { record("Free"); delete &session(hnd); return 0; }
|
||||
extern "C" int32_t X502_GetAdcFreq(t_x502_hnd hnd, double *f_acq, double *f_frame) { *f_acq = 2000000; *f_frame = 1000000; return 0; }
|
||||
extern "C" const char* X502_GetErrorString(int32_t err) { return "SDK contract fixture"; }
|
||||
extern "C" int32_t X502_GetRecvReadyCount(t_x502_hnd hnd, uint32_t *rdy_cnt) { *rdy_cnt = session(hnd).input.size(); return 0; }
|
||||
extern "C" int32_t X502_OutCycleLoadStart(t_x502_hnd hnd, uint32_t size) { record("OutCycleLoadStart"); return 0; }
|
||||
extern "C" int32_t X502_OutCycleSetup(t_x502_hnd hnd, uint32_t flags) { if (flags != X502_OUT_CYCLE_FLAGS_WAIT_DONE) { return -1; } record("OutCycleSetup WAIT_DONE"); return 0; }
|
||||
extern "C" int32_t X502_OutCycleStop(t_x502_hnd hnd, uint32_t flags) { if (flags != X502_OUT_CYCLE_FLAGS_WAIT_DONE) { return -1; } record("OutCycleStop WAIT_DONE"); return 0; }
|
||||
extern "C" int32_t X502_PrepareData(t_x502_hnd hnd, const double* dac1, const double* dac2, const uint32_t* digout, uint32_t size, int32_t flags, uint32_t* out_buf) { record("PrepareData"); std::copy_n(digout, size, out_buf); return 0; }
|
||||
extern "C" int32_t X502_ProcessData(t_x502_hnd hnd, const uint32_t* src, uint32_t size, uint32_t flags, double *adc_data, uint32_t *adc_data_size, uint32_t *din_data, uint32_t *din_data_size) { *adc_data_size = *din_data_size = size / 2;
|
||||
for (unsigned n = 0; n < size / 2; ++n) {
|
||||
adc_data[n] = static_cast<int32_t>(src[2 * n]); din_data[n] = src[2 * n + 1];
|
||||
}
|
||||
return 0; }
|
||||
extern "C" int32_t X502_Recv(t_x502_hnd hnd, uint32_t* buf, uint32_t size, uint32_t tout) { auto& s = session(hnd); usleep(20000);
|
||||
if (s.consumed) { raise(SIGTERM); return 0; }
|
||||
if (size < s.input.size()) { return -1; }
|
||||
std::copy(s.input.begin(), s.input.end(), buf); s.consumed = true;
|
||||
return static_cast<int32_t>(s.input.size()); }
|
||||
extern "C" int32_t X502_Send(t_x502_hnd hnd, const uint32_t* buf, uint32_t size, uint32_t tout) { record("Send"); return size; }
|
||||
extern "C" int32_t X502_SetAdcFreq(t_x502_hnd hnd, double *f_acq, double *f_frame) { *f_acq = 2000000; if (f_frame) { *f_frame = 1000000; } return 0; }
|
||||
extern "C" int32_t X502_SetDigInPullup(t_x502_hnd hnd, uint32_t pullups) { return 0; }
|
||||
extern "C" int32_t X502_SetDinFreq(t_x502_hnd hnd, double *f_din) { return 0; }
|
||||
extern "C" int32_t X502_SetLChannel(t_x502_hnd hnd, uint32_t lch, uint32_t phy_ch, uint32_t mode, uint32_t range, uint32_t avg) { return 0; }
|
||||
extern "C" int32_t X502_SetLChannelCount(t_x502_hnd hnd, uint32_t lch_cnt) { return 0; }
|
||||
extern "C" int32_t X502_SetMode(t_x502_hnd hnd, uint32_t mode) { return 0; }
|
||||
extern "C" int32_t X502_SetOutFreq(t_x502_hnd hnd, double *f_dout) { return 0; }
|
||||
extern "C" int32_t X502_SetRefFreq(t_x502_hnd hnd, uint32_t freq) { return 0; }
|
||||
extern "C" int32_t X502_SetStreamBufSize(t_x502_hnd hnd, uint32_t ch, uint32_t size) { return 0; }
|
||||
extern "C" int32_t X502_SetStreamStep(t_x502_hnd hnd, uint32_t dma_ch, uint32_t step) { return 0; }
|
||||
extern "C" int32_t X502_SetSyncMode(t_x502_hnd hnd, uint32_t sync_mode) { return 0; }
|
||||
extern "C" int32_t X502_SetSyncStartMode(t_x502_hnd hnd, uint32_t sync_start_mode) { return 0; }
|
||||
extern "C" int32_t X502_StreamsDisable(t_x502_hnd hnd, uint32_t streams) { record("StreamsDisable"); return 0; }
|
||||
extern "C" int32_t X502_StreamsEnable(t_x502_hnd hnd, uint32_t streams) { return 0; }
|
||||
extern "C" int32_t X502_StreamsStart(t_x502_hnd hnd) { record("Start"); if (session(hnd).id == 4) { raise(SIGTERM); } return 0; }
|
||||
extern "C" int32_t X502_StreamsStop(t_x502_hnd hnd) { record("StreamsStop"); return 0; }
|
||||
Binary file not shown.
@@ -0,0 +1,255 @@
|
||||
"""The sar3d processor on the real CUDA backend: notebook reference and contracts.
|
||||
|
||||
Every case runs build/native_tests/backprojection_fixture (BACKPROJECTION_FIXTURE overrides it).
|
||||
Without a usable GPU the processor publishes sar3d_status 3: then only that
|
||||
warning and the box rules, which are host code, are checked, and the remaining tests
|
||||
are skipped. The notebook is needed only to regenerate the references with
|
||||
generate_backprojection_reference.py.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import subprocess
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
FIXTURE = Path(os.environ.get("BACKPROJECTION_FIXTURE", ROOT / "build/native_tests/backprojection_fixture"))
|
||||
REFERENCE = Path(__file__).with_name("sar3d_reference.npz")
|
||||
VIEWS = ("sar3d_front", "sar3d_side", "sar3d_top", "sar3d_oblique")
|
||||
|
||||
|
||||
def run(request: dict | list[dict]) -> dict[str, dict]:
|
||||
output = subprocess.run([str(FIXTURE)], input=json.dumps(request), text=True,
|
||||
capture_output=True, timeout=30, check=True)
|
||||
result = json.loads(output.stdout)
|
||||
if isinstance(request, list):
|
||||
result = result[-1]
|
||||
return {item["name"]: item for item in result["payloads"]}
|
||||
|
||||
|
||||
def status(payloads: dict[str, dict]) -> tuple[int, str]:
|
||||
table = payloads["sar3d_status"]["table"]
|
||||
return int(table[0]), bytes(map(int, table[1:])).decode("utf-8")
|
||||
|
||||
|
||||
def reference_cases(archive) -> list[dict]:
|
||||
"""Requests of the reference archive, one per notebook parameter case."""
|
||||
cases = []
|
||||
for case in json.loads(archive["cases"].item()):
|
||||
request = json.loads(archive["request"].item())
|
||||
request.update(case["settings"])
|
||||
for member, windows in zip([*request["previous"], request["latest"]], case["windows"], strict=True):
|
||||
for trace, (start, end) in zip(member["traces"], windows, strict=True):
|
||||
trace.update(start=start, end=end)
|
||||
cases.append(request)
|
||||
return cases
|
||||
|
||||
|
||||
class Sar3dTest(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls) -> None:
|
||||
with np.load(REFERENCE, allow_pickle=False) as archive:
|
||||
cls.reference = {key: archive[key] for key in archive.files}
|
||||
cls.cases = reference_cases(cls.reference)
|
||||
cls.scene_payloads = run(cls.cases[0])
|
||||
code, message = status(cls.scene_payloads)
|
||||
cls.unavailable = message if code == 3 else None
|
||||
|
||||
def scene(self, **settings) -> dict:
|
||||
if self.unavailable:
|
||||
self.skipTest(self.unavailable)
|
||||
request = copy.deepcopy(self.cases[0])
|
||||
request.update(settings)
|
||||
return request
|
||||
|
||||
def test_unavailable_gpu_is_a_visible_warning(self) -> None:
|
||||
code, message = status(self.scene_payloads)
|
||||
if self.unavailable:
|
||||
self.assertIn("CUDA", message)
|
||||
self.assertFalse(any(item["image"] for item in self.scene_payloads.values()))
|
||||
else:
|
||||
self.assertEqual(code, 0, message)
|
||||
|
||||
def test_cuda_matches_notebook(self) -> None:
|
||||
if self.unavailable:
|
||||
self.skipTest(self.unavailable)
|
||||
for index, request in enumerate(self.cases):
|
||||
with self.subTest(case=index):
|
||||
payloads = run(request)
|
||||
self.assertEqual(status(payloads)[0], 0, status(payloads))
|
||||
for view, horizontal, vertical in (("front", "x", "y"), ("side", "z", "y"), ("top", "x", "z")):
|
||||
payload = payloads[f"sar3d_{view}"]
|
||||
expected = self.reference[f"case_{index}_{view}"]
|
||||
np.testing.assert_allclose(payload["x"], self.reference[f"case_{index}_{horizontal}"],
|
||||
rtol=2e-6, atol=1e-6)
|
||||
np.testing.assert_allclose(payload["y"], self.reference[f"case_{index}_{vertical}"],
|
||||
rtol=2e-6, atol=1e-6)
|
||||
np.testing.assert_allclose(np.asarray(payload["image"]).reshape(expected.shape),
|
||||
expected, rtol=2e-3, atol=3e-4)
|
||||
widths = self.reference.get(f"case_{index}_target_width")
|
||||
if widths is None:
|
||||
self.assertNotIn("sar3d_target_width", payloads)
|
||||
else:
|
||||
np.testing.assert_allclose(payloads["sar3d_target_width"]["table"], widths, rtol=2e-3, atol=2e-4)
|
||||
peak = payloads["sar3d_peak"]["table"]
|
||||
expected_peak = self.reference[f"case_{index}_peak"]
|
||||
np.testing.assert_allclose(peak[1:], expected_peak[1:], rtol=0, atol=2e-6)
|
||||
np.testing.assert_allclose(peak[0], expected_peak[0], rtol=2e-3, atol=0)
|
||||
self.check_display(index, payloads)
|
||||
|
||||
def check_display(self, index: int, payloads: dict[str, dict]) -> None:
|
||||
"""Background, CLEAN objects and boxes against sar_prepare_display.
|
||||
|
||||
Steps at the noise level may pick a neighbouring A-scan sample where two samples are
|
||||
equal to float32 precision; the reference scenes were checked to keep their decisions
|
||||
with the kernels compiled without fused multiply-adds.
|
||||
"""
|
||||
background = self.reference.get(f"case_{index}_background")
|
||||
if background is None:
|
||||
self.assertNotIn("sar3d_background", payloads)
|
||||
else:
|
||||
np.testing.assert_allclose(payloads["sar3d_background"]["table"], background, rtol=2e-3, atol=0)
|
||||
objects = self.reference.get(f"case_{index}_objects")
|
||||
if objects is None:
|
||||
self.assertNotIn("sar3d_objects", payloads)
|
||||
self.assertNotIn("sar3d_marks", payloads)
|
||||
else:
|
||||
table = np.reshape(payloads["sar3d_objects"]["table"], (-1, 4))
|
||||
np.testing.assert_allclose(table[:, :3], objects[:, :3], rtol=0, atol=2e-6)
|
||||
np.testing.assert_allclose(table[:, 3], objects[:, 3], rtol=2e-3, atol=0)
|
||||
np.testing.assert_allclose(np.reshape(payloads["sar3d_marks"]["table"], (-1, 7)),
|
||||
self.reference[f"case_{index}_marks"], rtol=0, atol=2e-6)
|
||||
clean = self.reference.get(f"case_{index}_clean")
|
||||
if clean is not None:
|
||||
message = status(payloads)[1]
|
||||
match = re.search(r"\((\d+) subtractions?, residual maximum ([\d.]+) backgrounds\)", message)
|
||||
assert match is not None, message
|
||||
self.assertEqual(int(match[1]), clean[0])
|
||||
self.assertAlmostEqual(float(match[2]), clean[1], delta=0.051)
|
||||
|
||||
def test_clean_needs_a_one_frame_aperture(self) -> None:
|
||||
# The actual aperture decides, as in sar_clean_frame: three frames skip CLEAN, while the
|
||||
# first frame of a run is a one-frame aperture even with SAR_FRAME_RADIUS = 2.
|
||||
skipped = run(self.scene(clean=True))
|
||||
code, message = status(skipped)
|
||||
self.assertEqual(code, 0, message)
|
||||
self.assertIn("CLEAN skipped: it needs a one-frame aperture, this one has 3 frames", message)
|
||||
self.assertNotIn("sar3d_objects", skipped)
|
||||
code, message = status(run(self.scene(clean=True, previous=[])))
|
||||
self.assertEqual(code, 0, message)
|
||||
self.assertIn("; CLEAN: ", message)
|
||||
|
||||
def test_boxes_follow_the_notebook_rules(self) -> None:
|
||||
# Host code only, so it runs without a GPU as well.
|
||||
request = json.loads(self.reference["marks_request"].item())
|
||||
output = subprocess.run([str(FIXTURE)], input=json.dumps(request), text=True,
|
||||
capture_output=True, timeout=30, check=True)
|
||||
np.testing.assert_allclose(json.loads(output.stdout)["marks"], self.reference["marks"], rtol=0, atol=1e-9)
|
||||
|
||||
def test_invalid_input_is_an_error_with_a_reason(self) -> None:
|
||||
def no_traces(request): request["latest"]["traces"] = []
|
||||
def empty_window(request): request["latest"]["traces"][0]["end"] = request["latest"]["traces"][0]["start"]
|
||||
def falling_frequency(request): request["latest"]["traces"][0]["frequency"].reverse()
|
||||
def overflow(request):
|
||||
for trace in request["latest"]["traces"]:
|
||||
samples = np.asarray(trace["s21"])
|
||||
trace["s21"] = (samples / np.abs(samples).max() * 1e38).tolist()
|
||||
for name, settings, damage in (
|
||||
("no traces", {}, no_traces),
|
||||
("empty capture window", {}, empty_window),
|
||||
("falling frequency", {}, falling_frequency),
|
||||
("volume beyond float32", {"previous": [], "radius": 0, "normalize": False, "equalize": False}, overflow),
|
||||
("grid beyond 32-bit indexing", {"voxel": [1e-8] * 3}, None),
|
||||
("vertical look angle", {"pitch": 90}, None),
|
||||
("result larger than the ring slot", {"slot": 1024}, None),
|
||||
):
|
||||
with self.subTest(name):
|
||||
request = self.scene(**settings)
|
||||
if damage:
|
||||
damage(request)
|
||||
code, message = status(run(request))
|
||||
self.assertEqual(code, 2, message)
|
||||
self.assertTrue(message)
|
||||
|
||||
def test_views_are_normalized_at_the_peak_of_the_reference_planes(self) -> None:
|
||||
# Fine grids spread every Y plane over all threads of the per-plane maximum.
|
||||
for voxel in (.05, .06, .07, .08, .09, .11):
|
||||
with self.subTest(voxel=voxel):
|
||||
payloads = run(self.scene(voxel=[voxel] * 3))
|
||||
self.assertEqual(status(payloads)[0], 0, status(payloads))
|
||||
front = payloads["sar3d_front"]
|
||||
x, y = np.asarray(front["x"]), np.asarray(front["y"])
|
||||
image = np.where(((y >= y[0] + .3) & (y <= y[-1] - .3))[:, None],
|
||||
np.reshape(front["image"], (len(y), len(x))), -1.)
|
||||
row, column = np.unravel_index(np.argmax(image), image.shape)
|
||||
self.assertAlmostEqual(float(image[row, column]), 1., places=5)
|
||||
np.testing.assert_allclose(payloads["sar3d_peak"]["table"][1:3], [x[column], y[row]], rtol=0, atol=1e-6)
|
||||
|
||||
def test_cached_aperture_updates_when_previous_mean_context_changes(self) -> None:
|
||||
request = self.scene(subtract_mean=True, mean_count=1)
|
||||
shorter = copy.deepcopy(request)
|
||||
shorter["previous"] = shorter["previous"][-1:]
|
||||
cached, fresh = run([request, shorter]), run(shorter)
|
||||
self.assertEqual(status(cached)[0], 0, status(cached))
|
||||
for name in VIEWS:
|
||||
np.testing.assert_allclose(cached[name]["image"], fresh[name]["image"], rtol=2e-3, atol=3e-4)
|
||||
|
||||
def test_empty_signal_is_reported_without_a_peak(self) -> None:
|
||||
request = self.scene(previous=[], radius=0)
|
||||
for trace in request["latest"]["traces"]:
|
||||
trace["s21"] = [[0, 0]] * len(trace["frequency"])
|
||||
payloads = run(request)
|
||||
self.assertEqual(status(payloads)[0], 1)
|
||||
self.assertNotIn("sar3d_peak", payloads)
|
||||
self.assertNotIn("sar3d_background", payloads)
|
||||
for name in VIEWS:
|
||||
self.assertFalse(np.any(payloads[name]["image"]))
|
||||
|
||||
def test_weak_signal_stays_finite(self) -> None:
|
||||
request = self.scene(previous=[], radius=0, normalize=False)
|
||||
for trace in request["latest"]["traces"]:
|
||||
trace["s21"] = (np.asarray(trace["s21"]) * 1e-38).tolist()
|
||||
payloads = run(request)
|
||||
self.assertEqual(status(payloads)[0], 0, status(payloads))
|
||||
for name in VIEWS:
|
||||
values = np.asarray(payloads[name]["image"])
|
||||
self.assertTrue(np.isfinite(values).all())
|
||||
self.assertGreater(values.max(), 0)
|
||||
self.assertLess(values.max(), 1e-10)
|
||||
|
||||
|
||||
class GprTest(unittest.TestCase):
|
||||
def test_changing_tx_rx_counts_rebuilds_the_cached_grid(self) -> None:
|
||||
with np.load(REFERENCE.with_name("gpr_reference.npz"), allow_pickle=False) as reference:
|
||||
base = json.loads(reference["case_0_request"].item())
|
||||
requests = []
|
||||
for tx, rx in ((2, 1), (1, 2)):
|
||||
request = copy.deepcopy(base)
|
||||
request.update(tx=[[i, 0, 0, 0] for i in range(tx)], rx=[[i, 0, 0, 0] for i in range(rx)])
|
||||
request["latest"]["traces"] = [trace for trace in request["latest"]["traces"]
|
||||
if trace["output"] < tx and trace["input"] < rx]
|
||||
requests.append(request)
|
||||
np.testing.assert_allclose(run(requests)["gpr_accumulator"]["image"],
|
||||
run(requests[-1])["gpr_accumulator"]["image"])
|
||||
|
||||
def test_coherent_ground_bp_matches_horns_and_notebook_equalizer(self) -> None:
|
||||
with np.load(REFERENCE.with_name("gpr_reference.npz"), allow_pickle=False) as reference:
|
||||
for i in range(3):
|
||||
with self.subTest(case=i):
|
||||
request = json.loads(reference[f"case_{i}_request"].item())
|
||||
payload = run(request)["gpr_accumulator"]
|
||||
expected = reference[f"case_{i}_image"]
|
||||
np.testing.assert_allclose(np.asarray(payload["image"]).reshape(expected.shape), expected,
|
||||
rtol=2e-4, atol=3e-5)
|
||||
np.testing.assert_allclose(payload["y"], np.linspace(.1, request["max_depth"], expected.shape[0]),
|
||||
rtol=1e-6, atol=1e-7)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Executable
+45
@@ -0,0 +1,45 @@
|
||||
#!/usr/bin/env bash
|
||||
# Put the project on a USB flash drive for deploy/update.sh: the host part of the
|
||||
# last commit as an archive (no venv, build, data or local configs) and the update
|
||||
# script, both in <drive>/radar_update/.
|
||||
#
|
||||
# deploy/pack-usb.sh [DRIVE]
|
||||
#
|
||||
# DRIVE defaults to the only mounted removable drive.
|
||||
set -euo pipefail
|
||||
|
||||
PROJECT_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# What runs on the Pi or Jetson; device_firmware/ stays on this machine.
|
||||
HOST_PATHS=(data_acq_and_processing deploy python_app run_config_examples Makefile
|
||||
"requirements.txt" run_config.json start.sh)
|
||||
|
||||
drive="${1:-}"
|
||||
if [[ -z "${drive}" ]]; then
|
||||
mapfile -t drives < <(lsblk -nlo HOTPLUG,MOUNTPOINT | sed -n 's|^ *1 \+\(/.*\)$|\1|p')
|
||||
if (( ${#drives[@]} != 1 )); then
|
||||
echo "Found ${#drives[@]} mounted removable drives; name one: $0 /path/to/drive" >&2
|
||||
printf ' %s\n' "${drives[@]}" >&2
|
||||
exit 1
|
||||
fi
|
||||
drive="${drives[0]}"
|
||||
fi
|
||||
if [[ ! -d "${drive}" || ! -w "${drive}" ]]; then
|
||||
echo "Not a writable directory: ${drive}" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
cd "${PROJECT_ROOT}"
|
||||
if [[ -n "$(git status --porcelain -- "${HOST_PATHS[@]}")" ]]; then
|
||||
echo "Note: uncommitted changes are left out, the archive holds the last commit." >&2
|
||||
fi
|
||||
release="radar_system-$(git log -1 --format=%cd --date=format:%Y%m%d-%H%M)-$(git rev-parse --short HEAD)"
|
||||
out="${drive}/radar_update"
|
||||
mkdir -p "${out}"
|
||||
git archive --format=tar.gz --prefix=radar_system/ -o "${out}/${release}.tar.gz" HEAD "${HOST_PATHS[@]}"
|
||||
git show HEAD:deploy/update.sh > "${out}/update.sh"
|
||||
sync
|
||||
|
||||
echo "Written to ${out}:"
|
||||
echo " ${release}.tar.gz"
|
||||
echo " update.sh"
|
||||
echo "On the device, from the drive's folder: bash radar_update/update.sh"
|
||||
Executable
+189
@@ -0,0 +1,189 @@
|
||||
#!/usr/bin/env bash
|
||||
# Install a release archive made by deploy/pack-usb.sh over the project on this
|
||||
# device. The code is replaced; configs, calibrations and data are kept; the current
|
||||
# version stays next to it as radar_system<N> (the next free number) for a rollback.
|
||||
#
|
||||
# bash update.sh [--python VERSION|PATH] [ARCHIVE]
|
||||
#
|
||||
# ARCHIVE the newest radar_system-*.tar.gz next to this script by default
|
||||
# --python build a fresh venv with this interpreter, e.g. 3.13 (found with uv or as
|
||||
# python3.13); without it the current venv is carried over
|
||||
# The project is radar_system in the Documents folder unless RADAR_DIR names it.
|
||||
set -Eeuo pipefail
|
||||
shopt -s nullglob
|
||||
|
||||
say() { echo "[update] $*"; }
|
||||
fail() { echo "[update] $*" >&2; exit 1; }
|
||||
|
||||
(( EUID != 0 )) || fail "Run it as your usual user, not with sudo: the project files must stay yours."
|
||||
if [[ -n "${RADAR_DIR:-}" ]]; then
|
||||
target="${RADAR_DIR}"
|
||||
else
|
||||
target="${HOME}/Documents/radar_system"
|
||||
localized="$(xdg-user-dir DOCUMENTS 2>/dev/null || true)/radar_system" # e.g. ~/Документы
|
||||
if [[ ! -e "${target}" && -d "${localized}" ]]; then target="${localized}"; fi
|
||||
fi
|
||||
[[ ! -L "${target}" ]] || fail "${target} is a symlink; set RADAR_DIR to the real directory."
|
||||
target="$(realpath -m -- "${target}")"
|
||||
script_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
|
||||
python_request=""
|
||||
archive=""
|
||||
while (( $# > 0 )); do
|
||||
case "$1" in
|
||||
--python) python_request="${2:?--python needs a version or a path}"; shift 2 ;;
|
||||
-h|--help) sed -n '2,12p' "${BASH_SOURCE[0]}"; exit 0 ;;
|
||||
*) archive="$1"; shift ;;
|
||||
esac
|
||||
done
|
||||
if [[ -n "${archive}" ]]; then archive="$(realpath -- "${archive}")"; fi
|
||||
cd "${HOME}" # never stand in the directory that is about to be renamed
|
||||
|
||||
# --- check everything before touching the project ------------------------------
|
||||
if [[ -z "${archive}" ]]; then
|
||||
releases=("${script_dir}"/radar_system-*.tar.gz)
|
||||
(( ${#releases[@]} > 0 )) || fail "No radar_system-*.tar.gz next to ${BASH_SOURCE[0]}."
|
||||
archive="${releases[-1]}" # the names sort by commit date
|
||||
fi
|
||||
listing="$(tar -tzf "${archive}")" || fail "Damaged archive: ${archive}"
|
||||
grep -qx 'radar_system/start.sh' <<< "${listing}" || fail "Not a radar_system release: ${archive}"
|
||||
if [[ ! -e "${target}" ]]; then
|
||||
read -r -p "[update] No project at ${target}. Install a fresh one there? [y/N] " answer
|
||||
[[ "${answer}" == [yY]* ]] || exit 1
|
||||
fi
|
||||
|
||||
python=""
|
||||
if [[ -n "${python_request}" ]]; then
|
||||
if [[ "${python_request}" =~ ^3\.[0-9]+$ ]]; then
|
||||
uv="$(command -v uv || echo "${HOME}/.local/bin/uv")"
|
||||
if [[ -x "${uv}" ]]; then
|
||||
python="$("${uv}" python find "${python_request}" 2>/dev/null || true)"
|
||||
fi
|
||||
[[ -n "${python}" ]] || python="$(command -v "python${python_request}" || true)"
|
||||
[[ -n "${python}" ]] || fail "Python ${python_request} not found; install it: uv python install ${python_request}"
|
||||
else
|
||||
python="$(command -v -- "${python_request}" || true)"
|
||||
[[ -n "${python}" ]] || fail "Interpreter not found: ${python_request}"
|
||||
fi
|
||||
say "venv interpreter: ${python} ($("${python}" --version 2>&1))"
|
||||
fi
|
||||
|
||||
parent="$(dirname -- "${target}")"
|
||||
mkdir -p -- "${parent}"
|
||||
staging="$(mktemp -d "${parent}/.radar_update.XXXXXX")"
|
||||
trap 'rm -rf -- "${staging}"' EXIT
|
||||
tar -xzf "${archive}" -C "${staging}"
|
||||
|
||||
# --- stop everything that runs from the project ----------------------------------
|
||||
daemon_stopped=0
|
||||
if command -v systemctl >/dev/null && systemctl is-active --quiet radar.service; then
|
||||
say "stopping radar.service"
|
||||
sudo systemctl stop radar.service
|
||||
daemon_stopped=1
|
||||
fi
|
||||
start_daemon() {
|
||||
if (( daemon_stopped )); then
|
||||
say "starting radar.service"
|
||||
sudo systemctl start radar.service
|
||||
fi
|
||||
}
|
||||
busy=()
|
||||
for proc in /proc/[0-9]*; do
|
||||
pid="${proc#/proc/}"
|
||||
[[ "${pid}" == "$$" || "${pid}" == "${PPID}" ]] && continue
|
||||
cwd="$(readlink "${proc}/cwd" 2>/dev/null)" || continue
|
||||
[[ "${cwd}" == "${target}" || "${cwd}" == "${target}/"* ]] && busy+=("${pid}")
|
||||
done
|
||||
if (( ${#busy[@]} > 0 )); then
|
||||
echo "[update] The program still runs from ${target}; close it and run the update again:" >&2
|
||||
ps -o pid=,args= -p "$(IFS=,; echo "${busy[*]}")" >&2 || true
|
||||
start_daemon
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# --- swap the versions --------------------------------------------------------------
|
||||
backup=""
|
||||
if [[ -e "${target}" ]]; then
|
||||
n=1
|
||||
while [[ -e "${target}${n}" ]]; do n=$((n + 1)); done
|
||||
backup="${target}${n}"
|
||||
mv -- "${target}" "${backup}"
|
||||
on_failure() {
|
||||
echo "[update] FAILED. The previous version is ${backup}; to go back to it:" >&2
|
||||
echo " rm -rf '${target}' && mv '${backup}' '${target}'" >&2
|
||||
if (( daemon_stopped )); then echo " sudo systemctl start radar.service" >&2; fi
|
||||
}
|
||||
trap on_failure ERR
|
||||
fi
|
||||
mv -- "${staging}/radar_system" "${target}"
|
||||
say "installed ${archive##*/} into ${target}"
|
||||
|
||||
# --- keep this device's state ----------------------------------------------------------
|
||||
if [[ -n "${backup}" ]]; then
|
||||
say "previous version: ${backup}"
|
||||
kept=()
|
||||
# The active profile; the release carries the development one (mock driver).
|
||||
if [[ -f "${backup}/run_config.json" ]]; then
|
||||
cp -a -- "${backup}/run_config.json" "${target}/run_config.json"
|
||||
kept+=(run_config.json)
|
||||
fi
|
||||
# Profiles changed or added on this device win; a changed one gets the release
|
||||
# version next to it as .new.
|
||||
renamed=()
|
||||
for profile in "${backup}"/run_config_examples/*.json; do
|
||||
shipped="${target}/run_config_examples/${profile##*/}"
|
||||
if [[ -f "${shipped}" ]]; then
|
||||
cmp -s -- "${profile}" "${shipped}" && continue
|
||||
mv -- "${shipped}" "${shipped}.new"
|
||||
renamed+=("${profile##*/}")
|
||||
fi
|
||||
cp -a -- "${profile}" "${shipped}"
|
||||
kept+=("run_config_examples/${profile##*/}")
|
||||
done
|
||||
# Runtime state (last profile, live settings), without logs and pid files.
|
||||
if [[ -d "${backup}/python_app/runtime" ]]; then
|
||||
cp -a -- "${backup}/python_app/runtime" "${target}/python_app/runtime"
|
||||
rm -rf -- "${target}/python_app/runtime/logs" "${target}/python_app/runtime/"*.pids
|
||||
kept+=(python_app/runtime)
|
||||
fi
|
||||
# Data: calibrations, references, snapshots, and every top-level folder that is
|
||||
# not part of the release. Hard links are instant and take no space; the program
|
||||
# writes new files instead of rewriting old ones.
|
||||
data=()
|
||||
for entry in "${backup}"/python_app/data*; do data+=("python_app/${entry##*/}"); done
|
||||
for entry in "${backup}"/*; do
|
||||
name="${entry##*/}"
|
||||
case "${name}" in build|graphify-out|device_firmware|docs|RELEASE) continue ;; esac
|
||||
[[ -e "${target}/${name}" ]] || data+=("${name}")
|
||||
done
|
||||
for entry in "${data[@]}"; do
|
||||
cp -al -- "${backup}/${entry}" "${target}/${entry}"
|
||||
kept+=("${entry}")
|
||||
done
|
||||
say "kept from ${backup##*/}: ${kept[*]:-nothing}"
|
||||
if (( ${#renamed[@]} > 0 )); then
|
||||
say "profiles changed on this device are kept, the release ones are next to them as .new: ${renamed[*]}"
|
||||
fi
|
||||
fi
|
||||
|
||||
# --- Python environment and C++ build ------------------------------------------------
|
||||
venv="${target}/.venv"
|
||||
if [[ -z "${python}" && -n "${backup}" && -x "${backup}/.venv/bin/python" ]]; then
|
||||
say "carrying the venv over"
|
||||
# pip and generated caches may rewrite existing files: the rollback copy
|
||||
# must own its files independently of the environment being upgraded.
|
||||
cp -a -- "${backup}/.venv" "${venv}"
|
||||
"${venv}/bin/python" -m pip install --quiet -r "${target}/requirements.txt"
|
||||
else
|
||||
python="${python:-$(command -v python3)}"
|
||||
say "creating a venv with ${python} (downloads the packages)"
|
||||
"${python}" -m venv "${venv}"
|
||||
"${venv}/bin/python" -m pip install --upgrade pip
|
||||
"${venv}/bin/python" -m pip install -r "${target}/requirements.txt"
|
||||
fi
|
||||
say "building the C++ part"
|
||||
make -C "${target}" -j"$(nproc)" all rph_radar_collector
|
||||
|
||||
echo "${archive##*/}" > "${target}/RELEASE"
|
||||
trap - ERR
|
||||
start_daemon
|
||||
say "done ($("${venv}/bin/python" --version 2>&1)). Start it: cd '${target}' && ./start.sh"
|
||||
@@ -8,15 +8,7 @@ static const int X_FWD = 650, X_MID = 2014, X_BACK = 3378; // PA0 (плечи
|
||||
static const int Y_RIGHT = 479, Y_MID = 1981, Y_LEFT = 3586; // PA1
|
||||
static const int DEADZONE = 15; // отсечка дребезга вокруг нейтрали
|
||||
|
||||
// ---- SBUS-значения старого пульта ----
|
||||
static const uint16_t SBUS_MID = 1024;
|
||||
static const uint16_t CH2_MIN = 433, CH2_MAX = 1643; // назад..вперёд
|
||||
static const uint16_t CH4_MIN = 446, CH4_MAX = 1654; // влево..вправо
|
||||
|
||||
// ---- expo: 0 = линейно, 1 = максимально мягкая нейтраль ----
|
||||
static const float EXPO_K = 0.0f;
|
||||
// ---- общий масштаб выхода: 1.0 = диапазон старого пульта ----
|
||||
static const float RANGE_SCALE = 0.75f;
|
||||
|
||||
// симметричные плечи: вперёд и назад дают одинаковый максимум
|
||||
static const uint16_t CH2_SPAN = 591; // min(1643-1024, 1024-433)
|
||||
@@ -75,15 +67,6 @@ static float axisNorm(int adc, int lowEnd, int mid, int highEnd) {
|
||||
return constrain(x, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
// expo-кривая: гасит чувствительность у нейтрали, сохраняет края
|
||||
static float expo(float x) {
|
||||
return EXPO_K * x * x * x + (1.0f - EXPO_K) * x;
|
||||
}
|
||||
|
||||
static uint16_t toSbus(float x, uint16_t span) {
|
||||
return (uint16_t)(SBUS_MID + lroundf(x * span));
|
||||
}
|
||||
|
||||
// стик [-1..1] -> SBUS-значение канала: ниже MOVE_START — нейтраль, выше —
|
||||
// линейно от порога срабатывания приёмника (db) до POWER_FRAC*span на полном стике
|
||||
static uint16_t driveToSbus(float x, uint16_t span, uint16_t db,
|
||||
@@ -129,11 +112,11 @@ void setup() {
|
||||
}
|
||||
|
||||
void loop() {
|
||||
static uint32_t next = 0;
|
||||
static uint32_t last = millis() - FRAME_PERIOD_MS;
|
||||
uint32_t now = millis();
|
||||
if (now < next)
|
||||
if (uint32_t(now - last) < FRAME_PERIOD_MS)
|
||||
return;
|
||||
next = now + FRAME_PERIOD_MS;
|
||||
last = now;
|
||||
|
||||
int adcX = readAvg(PIN_X);
|
||||
int adcY = readAvg(PIN_Y);
|
||||
|
||||
@@ -90,8 +90,9 @@ multi-device: `python_app/hardware_full/librevna_driver/protocol/frame.py`,
|
||||
Структура — `Protocol.hpp:186`: `uint32 ExtRefOuputFreq` плюс битовое поле
|
||||
`AutomaticSwitch` и `UseExternalRef`.
|
||||
|
||||
Шлём `output_frequency_hz = 0` и флаг внешней опоры: выход `Ref Out` не
|
||||
включаем, а устройству принудительно указываем использовать внешние 10 МГц.
|
||||
Шлём `output_frequency_hz = 0` (выход `Ref Out` не включаем) и `UseExternalRef` из
|
||||
`multi_device.force_external_reference` профиля; `AutomaticSwitch` всегда 0. Во всех
|
||||
профилях сейчас `false`, то есть приборы работают от внутренней опоры.
|
||||
Строит `build_reference_settings_payload` (`protocol.py:48`), отправляет
|
||||
`controller.py`.
|
||||
|
||||
@@ -208,16 +209,108 @@ ref» — сигнал на первом приёмнике.
|
||||
отсчёты приёмников. Считает хост, в
|
||||
`librevna_multi_device_driver/cycle_collection.py`.
|
||||
|
||||
Нормировка (`calculate_last_cycle_s_parameters`, строка 783): каждый канал
|
||||
делится на опорный приёмник **мастера**, а не на локальный опорный приёмник
|
||||
слейва — потому что излучает именно мастер. Опорные измерения хранятся по
|
||||
номеру выходного канала (`master_reference_measurements_by_output`), так как с
|
||||
переключением стимула выходов больше, чем портов мастера.
|
||||
Нормировка (`calculate_s_parameters`): каждый канал прохождения
|
||||
делится на опорный приёмник **мастера**, а не на локальную опору слейва,
|
||||
потому что излучает мастер. Опорные измерения хранятся по выходному каналу
|
||||
(`master_reference_measurements_by_output`): при переключении стимула выходов
|
||||
больше, чем физических портов мастера.
|
||||
|
||||
Имена S-параметров: отражение мастера — `master_reflection_s_parameter_name`
|
||||
(строка 760), прямые каналы — `build_forward_s_parameter_names` (строка 765).
|
||||
Нумерация приёмных портов слейвов начинается **после** всех выходов, чтобы
|
||||
`S33`/`S44` отражения не столкнулись с прямыми каналами.
|
||||
Отражения мастера приложение не собирает и не использует для отбраковки циклов.
|
||||
Ключ канала — пара `(input_pos, output_pos)` с нумерацией от нуля. Входы слейвов
|
||||
идут подряд: два порта первого слейва, затем два порта второго. Выход равен
|
||||
стадии мастера. Эти же физические номера используются в `ComboKey` коллекции.
|
||||
|
||||
Одиночный LibreVNA использует Python-драйвер `librevna_driver`: порт 1 излучает
|
||||
на стадии 0, порт 2 выключен (`port2Stage = 7`), `port3Stage = 0` означает
|
||||
отсутствие под-стадий. Хост сохраняет S21 = отсчёт порта 2 / опорный отсчёт
|
||||
той же стадии. Опора распознаётся по биту Reference даже при маске `0x13`;
|
||||
она никогда не подменяет сигнальный отсчёт из-за одновременно выставленных
|
||||
битов портов. Физические номера антенных ключей остаются заданными конфигом.
|
||||
|
||||
Бюджет ожидания одиночного свипа зависит от числа точек, `1 / IFBW` и dwell,
|
||||
с запасом на установление частоты и обмен по USB. Отдельный тайм-аут отсутствия
|
||||
новых точек прерывает зависший прибор независимо от общего бюджета свипа.
|
||||
|
||||
В multi-device `collect_complete_running_sweep_cycle` собирает один цикл.
|
||||
Окно коллекции и всех её трасс берётся по получению от мастера точки 0 и
|
||||
последней принятой точки этого цикла. Метка точки 0 фиксируется до ожидания
|
||||
слейвов на барьере синхронизации. Слив очередей, ожидание начала цикла и
|
||||
повторные попытки сбора в окно не входят.
|
||||
|
||||
Внутри свипа индекс точки только растёт, поэтому цикл прибора закрывается на
|
||||
первой точке с неувеличившимся индексом — начале следующего свипа, даже если его
|
||||
точка 0 потеряна. Точки следующего свипа в цикл не попадают. Потерянные точки
|
||||
цикла становятся нулём; если таких больше 5 % трассы, цикл собирается заново.
|
||||
|
||||
### Формат коллекций между процессами хоста
|
||||
|
||||
Это отдельный формат от USB-пакетов прибора. `RAW3` и `PRP3` — четыре ASCII-байта
|
||||
magic (`0x33574152` и `0x33505250` little-endian). В каждой трассе идут массив
|
||||
float32 частот и массив complex64 S21: 12 байт на точку. Окна съёма коллекции
|
||||
и отдельных трасс обязательны: после массивов идут два u64 окна коллекции,
|
||||
u32 числа трасс и по два u64 на каждую трассу. Усечённый хвост отвергается,
|
||||
нулевые окна разрешены в бандлах калибровки/референса. Полный размер кадра:
|
||||
`44 + число_пар × (28 + 12 × число_точек)` байт. Старые RAW2/PPR2 не принимаются;
|
||||
runtime-бандлы пересобираются из NPZ при Start. Старые NPZ с лишними S11-массивами
|
||||
читаются по S21 без изменения файлов. Формат пакетов LibreVNA этим не меняется.
|
||||
|
||||
Кадр результата — `RSL3` (`0x334c5352`, ASCII little-endian). Его заголовок
|
||||
занимает 64 байта, все поля little-endian, без выравнивающих промежутков:
|
||||
|
||||
| Смещение | Поле | Тип |
|
||||
| --- | --- | --- |
|
||||
| 0 | magic | u32 |
|
||||
| 4 | collection_id | u64 |
|
||||
| 12 | monotonic_ns (завершение обработки) | u64 |
|
||||
| 20 | processing_duration_ns | u64 |
|
||||
| 28 | settings_revision | u64 |
|
||||
| 36 | capture_start_ns | u64 |
|
||||
| 44 | capture_end_ns | u64 |
|
||||
| 52 | speed_m_s | f32 |
|
||||
| 56 | число payload коллекции | u32 |
|
||||
| 60 | число блоков пар | u32 |
|
||||
|
||||
После заголовка идут payload'ы коллекции и блоки пар. Блок содержит номера
|
||||
входа и выхода, число payload'ов (три u32), затем сами payload'ы. Каждый payload
|
||||
начинается с типа u8 и имени (длина u16 и байты UTF-8); далее идут данные типа:
|
||||
комплексная трасса, скаляр f32, изображение f32 или таблица f32. Идентичность
|
||||
свипа на всех стадиях — `(collection_id, capture_start_ns)`, время стадии её
|
||||
не меняет.
|
||||
RSL2 отвергается; Python и C++ обновляются одновременно после остановки процессов.
|
||||
|
||||
### Живые настройки backprojection
|
||||
|
||||
GUI публикует настройки через `processing_live.json`; формат кадров колец от
|
||||
изменения настроек не меняется. Для coherent GPR и SAR `pair_depth_equalize`
|
||||
(boolean, по умолчанию true) включает `equalize_pairs_by_depth` перед медианной
|
||||
нормировкой пары. `gpr_background_subtract_enabled` и `gpr_background_mean_count`
|
||||
действуют в обоих режимах: среднее предыдущих коллекций вычитается до IFFT.
|
||||
Плоскость coherent GPR — земля с высотой `gpr_bp_ground_depth_m` и наклоном
|
||||
`gpr_look_angle_deg`. SAR использует `sar3d_frame_radius` предыдущих кадров и
|
||||
текущий; шаги вокселя по X, Y и Z — `sar3d_voxel_x_m`, `sar3d_voxel_y_m`, `sar3d_voxel_z_m`. Размеры сцены: `sar3d_x_size_m`, `sar3d_z_size_m` (0 — автоматически),
|
||||
`sar3d_y_min_m`, `sar3d_y_max_m` — наземные координаты, независимо от depth gate.
|
||||
`sar3d_bg_gate_enabled` (BG_GATE_ENABLE) и `sar3d_clean_enabled` (SAR_CLEAN_ENABLE),
|
||||
boolean, по умолчанию true: отношение максимума кадра к его фону и CLEAN апертуры
|
||||
из одного кадра.
|
||||
|
||||
SAR публикует `sar3d_peak` как TableF32 [уровень нормировки,X,Y,Z] отмеченного
|
||||
максимума и, если объектов CLEAN нет, `sar3d_target_width` с тремя колонками [X,Y,Z]:
|
||||
ширины непрерывного отклика на половине максимума по осевым сечениям через этот
|
||||
максимум сглаженного объёма (метры, не меньше одного вокселя). GUI использует ширины
|
||||
для квадратного контура цели. Без сигнала (статус 1) этих таблиц нет. Остальные
|
||||
таблицы TableF32:
|
||||
|
||||
- `sar3d_background` — [максимум/фон, BG_GATE_3D_SHOW, BG_GATE_3D_FULL], только при
|
||||
`sar3d_bg_gate_enabled` и сигнале; множитель шкалы считает GUI с порогом облака.
|
||||
- `sar3d_objects` — объекты CLEAN, сильнейший первым, строки [X,Y,Z,уровень в фонах].
|
||||
Если они есть, проекции — остаток в долях максимума кадра с каждым объектом в своей
|
||||
шкале 0–1, а `sar3d_peak` указывает на первый объект.
|
||||
- `sar3d_marks` — рамки групп объектов, строки [класс,X0,X1,Y0,Y1,Z0,Z1]; класс 0 —
|
||||
очень низкий, 1 — на земле, 2 — выше земли, 3 — у границы сцены.
|
||||
|
||||
Зум, `SAR_CLOUD_THRESHOLD` и `SAR_MARK_ENABLE` — настройки отображения desktop/веба;
|
||||
в live-файл не входят, ревизию и расчёт не меняют. RSL3 не меняется: таблицы
|
||||
используют существующий тип payload.
|
||||
|
||||
## 7. Почему поле мощности читается, но не используется
|
||||
|
||||
|
||||
@@ -21,13 +21,22 @@
|
||||
| Стадия | Порт мастера | Ключ | Выход |
|
||||
|---|---|---|---|
|
||||
| 0 | 1 | A | 0 |
|
||||
| 1 | 1 | B | 2 |
|
||||
| 2 | 2 | A | 1 |
|
||||
| 1 | 1 | B | 1 |
|
||||
| 2 | 2 | A | 2 |
|
||||
| 3 | 2 | B | 3 |
|
||||
|
||||
Стадии нумеруются **порт-мажорно**: старшие биты счётчика стадий выбирают порт,
|
||||
младший — положение ключа. Выходы нумеруются **позиция-мажорно**, поэтому выходы
|
||||
0 и 1 сохраняют тот же смысл, что и без переключения.
|
||||
И стадии, и выходы нумеруются **порт-мажорно**: старшие биты счётчика стадий
|
||||
выбирают порт, младший — положение ключа. **Номер выхода равен номеру стадии**,
|
||||
поэтому выходы идут ровно в том порядке, в котором прибор их и меряет — а это тот
|
||||
же порядок, который видно осциллографом на линии ключа при развешивании антенн.
|
||||
|
||||
> **Смена нумерации 2026-09-17.** До этой даты выходы нумеровались
|
||||
> позиция-мажорно: выходы 0 и 1 сохраняли смысл из стокового режима, но средние
|
||||
> два оказывались переставлены относительно физического порядка — слоты 0,1,2,3
|
||||
> попадали на выходы 0,2,1,3. Физический порядок подтверждён замером. Данные,
|
||||
> снятые до смены, **несопоставимы** с новыми: выходы 1 и 2 поменялись смыслом,
|
||||
> поэтому калибровки и референсы надо переснять. Выход 1 теперь означает
|
||||
> «порт 1, положение B», а не «порт 2, положение A».
|
||||
|
||||
Из этого следует главное свойство: **положение ключа — это младший бит номера
|
||||
стадии**, а период 2 нацело делит 4 стадии. Значит через границу точки чередование
|
||||
@@ -35,6 +44,11 @@
|
||||
цикл триггера — абсолютный счётчик стадий не нужен, нужна только правильная
|
||||
чётность, а её контроллер восстанавливает на каждой точке по длине паузы.
|
||||
|
||||
Приложение сохраняет только прохождение от мастера к приёмникам слейвов.
|
||||
Отражения мастера не собираются. Канал хранится как физическая пара
|
||||
`(input, output)`: входы 0 и 1 — порты первого слейва, 2 и 3 — второго;
|
||||
выход — номер стадии из таблицы.
|
||||
|
||||
## Контракт по проводу
|
||||
|
||||
Хост отправляет это в `build_sweep_settings_payload`, прошивка читает — обе
|
||||
@@ -169,22 +183,26 @@ VHDL держите в чистом ASCII: GHDL отвергает не-ASCII с
|
||||
снял 8 коллекций по 16 трасс:
|
||||
|
||||
```text
|
||||
выход 0: -71.5 дБ выходы 0 и 1: расхождение 0.5 дБ
|
||||
выход 1: -72.0 дБ выходы 2 и 3: расхождение 1.3 дБ
|
||||
выход 2: -70.6 дБ
|
||||
выход 3: -71.9 дБ
|
||||
порт 1, положение A: -71.5 дБ порт 1, A против B: расхождение 0.9 дБ
|
||||
порт 1, положение B: -70.6 дБ порт 2, A против B: расхождение 0.1 дБ
|
||||
порт 2, положение A: -72.0 дБ
|
||||
порт 2, положение B: -71.9 дБ
|
||||
```
|
||||
|
||||
(Числа те же, что были сняты; подписаны по портам и положениям, а не по номерам
|
||||
выходов, чтобы не зависеть от смены нумерации 2026-09-17. Пары для сравнения —
|
||||
один порт в двух положениях, а не соседние индексы.)
|
||||
|
||||
Что именно этим доказано:
|
||||
|
||||
- **Битстрим в приборе доработанный.** Со стоковым сравнением «стадия равна
|
||||
порт-стадии» излучали бы только стадии 0 и 2, а выходы 1 и 3 провалились бы в
|
||||
шум. Здесь все четыре на одном уровне.
|
||||
порт-стадии» излучали бы только стадии 0 и 2, то есть положение A каждого
|
||||
порта, а положение B провалилось бы в шум. Здесь все четыре на одном уровне.
|
||||
- **Петля триггера с контроллером замкнута.** При `syncMode = 3` мастер семплирует
|
||||
по вернувшемуся триггеру, так что без исправного зеркалирования в STM32 данных
|
||||
не было бы вовсе.
|
||||
- **Выходы попарно совпадают**, как и должно быть при отключённом ключе: обе его
|
||||
позиции — физически один путь.
|
||||
- **Положения одного порта совпадают** (0.9 и 0.1 дБ), как и должно быть при
|
||||
отключённом ключе: обе его позиции — физически один путь.
|
||||
|
||||
Считать фронты для этой проверки **бессмысленно**: поле `stages` стоковое, сток
|
||||
тоже отработает четыре стадии на точку, просто без стимула в 1 и 3.
|
||||
@@ -202,20 +220,30 @@ VHDL держите в чистом ASCII: GHDL отвергает не-ASCII с
|
||||
I2C, останавливает развёртку и даёт нерегулярные длинные паузы. Верхняя граница
|
||||
200 МГц лежит в полосе осциллографа, поэтому стимул видно напрямую.
|
||||
|
||||
Медиана `|S21|` по трём повторам, дБ:
|
||||
Подключён был **один** тракт: вход слейва 1 (вход 0) в порт 1 мастера через ключ.
|
||||
Остальные 15 комбинаций законно лежали на уровне шума −69…−78 дБ.
|
||||
|
||||
| вход | выход 0 (A) | выход 1 (B) | выход 2 (A) | выход 3 (B) |
|
||||
|---|---|---|---|---|
|
||||
| 0 | **−5.4** | −77.1 | −69.8 | −78.0 |
|
||||
| 1 | −69.2 | −68.9 | −69.0 | −69.1 |
|
||||
| 2 | −77.1 | −77.7 | −76.4 | −77.9 |
|
||||
| 3 | −73.6 | −73.8 | −72.8 | −73.5 |
|
||||
Медиана `|S21|` для входа 0, дБ, по портам и положениям ключа:
|
||||
|
||||
Подключён был один тракт — вход 0 в выход 0. Разница между двумя положениями
|
||||
одного и того же порта мастера (выходы 0 и 1) составила **72 дБ**: в положении A
|
||||
сигнал идёт в подключённый кабель, в положении B — в открытый порт. Это и есть
|
||||
прямое доказательство, что ключ переключается по стадиям. Повторяемость
|
||||
0.0–1.8 дБ, на сигнальном тракте 0.0 дБ.
|
||||
| порт мастера | положение A | положение B |
|
||||
|---|---|---|
|
||||
| порт 1 | −69.8 | **−5.2** |
|
||||
| порт 2 | −77.1 | −77.6 |
|
||||
|
||||
Разница между двумя положениями **одного и того же** порта 1 — **62–64 дБ**: в
|
||||
одном положении сигнал идёт в подключённый кабель, в другом в открытый выход. Это
|
||||
и есть прямое доказательство, что ключ переключается по стадиям. Повторяемость
|
||||
0.0–1.8 дБ, на сигнальном тракте 0.0 дБ, и положение не переехало за **30 свипов
|
||||
подряд** (`parity corrections: 0` на 200 тысячах измерений).
|
||||
|
||||
> Два предупреждения к этим числам. Первое: измерение сделано **до** смены
|
||||
> нумерации выходов, поэтому в логах того дня сигнал стоит на «выходе 2» — в
|
||||
> нынешней нумерации это выход **1**. Здесь таблица приведена по портам и
|
||||
> положениям, чтобы от нумерации не зависеть. Второе: в первой редакции этого
|
||||
> раздела я сравнивал выходы 0 и 1 и называл разницу между ними доказательством
|
||||
> переключения. Это было **неверно** — при старой нумерации соседние индексы были
|
||||
> разными портами, а не двумя положениями одного. Вывод подтвердился, но на
|
||||
> правильной паре.
|
||||
|
||||
**Ограничение по IFBW.** При `if_bandwidth_hz = 1000` прибор воспроизводимо
|
||||
падает: `LIBUSB_ERROR_IO`, затем устройства отваливаются с шины
|
||||
@@ -381,9 +409,13 @@ GUI сам разложит битстрим и прошивку MCU.
|
||||
два периода `A B A B`, PB2 и PB6 меняются одновременно и в одну сторону, PB5
|
||||
молчит.
|
||||
2. По USB CDC контроллера (развёртку остановить): `parity corrections : 0` и
|
||||
`stages per point : 4`.
|
||||
3. Сверить трейсы: выходы 0 и 1 должны совпасть с тем, что даёт старый режим —
|
||||
это те же порт 1 и порт 2 в положении A.
|
||||
`stages per resync` около `число точек x 4`. Строку про «stages per point» из
|
||||
старых версий прошивки не искать, её больше нет — см.
|
||||
[README контроллера](switch_controller/README.md).
|
||||
3. Сверить трейсы: **выходы 0 и 2** должны совпасть с тем, что даёт старый режим —
|
||||
это порт 1 и порт 2 в положении A. Выходы 1 и 3 — те же порты в положении B,
|
||||
их в старом режиме не было. (До смены нумерации 2026-09-17 стоковыми были
|
||||
выходы 0 и 1.)
|
||||
|
||||
## Хост
|
||||
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,203 @@
|
||||
# Устройство программы
|
||||
|
||||
## Процессы и кольца
|
||||
|
||||
Программа — четыре процесса (с АЦП E-502 — пять: добавляется коллектор), связанные
|
||||
кольцевыми буферами в разделяемой памяти POSIX:
|
||||
|
||||
```text
|
||||
raw preprocessed results
|
||||
железо ─► процесс сбора ─────► data_preprocessor ─────────► data_processor ─────► окно
|
||||
│ │ │
|
||||
│ raw_tap │ preprocessed_tap └─► TCP-локатор
|
||||
└─────► окно └─────► окно
|
||||
```
|
||||
|
||||
| Процесс | Что делает | Код |
|
||||
| --- | --- | --- |
|
||||
| окно программы (Python, PyQt6) | запускает и перезапускает остальные процессы, читает результаты и копии, рисует, обслуживает веб-интерфейс и GPIO-кнопку | `python_app/gui/` |
|
||||
| процесс сбора (Python) | управляет железом, публикует коллекции в `raw` | `python_app/scripts/matrix_raw_producer.py`, `rph_radar_raw_producer.py` |
|
||||
| препроцессор (C++) | калибровка, вычитание референса, режекторный фильтр, калибровка по объекту | `data_acq_and_processing/preprocessing/` |
|
||||
| процессор (C++) | режимы обработки, локатор | `data_acq_and_processing/processing/` |
|
||||
|
||||
Процесс сбора выбирается по `radar.model` в
|
||||
`python_app/orchestration/producer_selection.py`; той же функцией пользуются окно и
|
||||
`start.sh --producer-only`. У радиофотонного радара с АЦП E-502 процесс сбора
|
||||
запускает ещё и C++-коллектор `rph_radar_collector`, который передаёт развёртки через
|
||||
псевдотерминал.
|
||||
|
||||
Кольца работают по правилу «побеждает новое»: писатель никогда не ждёт читателя, при
|
||||
переполнении затирается самый старый непрочитанный кадр, а читатель, которого
|
||||
обогнали во время чтения, продолжает с самого свежего. Окно читает только копии
|
||||
`raw_tap` и `preprocessed_tap`: если бы оно читало основные кольца, оно отбирало бы
|
||||
кадры у препроцессора и процессора. Пустое имя копии отключает её.
|
||||
|
||||
Раскладка кольца — `RDRRING2`, версия 1: заголовок 64 байта, у каждого слота заголовок
|
||||
16 байт. Код: Python — `python_app/orchestration/shm/ring_layout.py`, C++ —
|
||||
`common_cpp/ipc/` (`ShmRing`). Владелец заменяет несовместимый сегмент через unlink, не
|
||||
обрезая отображения читателей.
|
||||
|
||||
## Форматы кадров
|
||||
|
||||
| Кадр | Где | Содержимое |
|
||||
| --- | --- | --- |
|
||||
| RAW3 | `raw`, `raw_tap`, файлы наборов `python_app/runtime/*_bundle.bin` | коллекция: окно съёма, затем для каждой трассы пара, окно съёма, частоты float32 и S21 complex64. Размер — 44 + пары × (28 + 12 × точки) байт |
|
||||
| PRP3 | `preprocessed`, `preprocessed_tap` | то же после предобработки |
|
||||
| RSL3 | `results` | заголовок 64 байта (номер коллекции, метка стадии, время обработки, `settings_revision`, окно съёма, использованная скорость, число результатов) и результаты: трасса, число, изображение или таблица, для всей коллекции и для пар |
|
||||
|
||||
Окно съёма обязательно; старые форматы RAW2, PRP2 и RSL2 отвергаются. Python и C++
|
||||
обновляются только вместе. Подробное описание полей — §6
|
||||
[librevna_protocol.md](../device_firmware/librevna_protocol.md).
|
||||
|
||||
## Идентичность и порядок коллекций
|
||||
|
||||
- Коллекция определяется парой `(collection_id, capture_start_ns)`. Номер после
|
||||
**Stop** и **Start** снова начинается с 1, поэтому одного номера мало.
|
||||
- Окно съёма ставит процесс сбора по часам CLOCK_MONOTONIC; дальше оно не меняется.
|
||||
У каждой трассы своё окно: при переключении пар окно коллекции про отдельную пару
|
||||
ничего не говорит.
|
||||
- История упорядочена по `capture_start_ns`. `monotonic_ns` — диагностическая метка
|
||||
стадии.
|
||||
- Сопоставление стадий, **Single Capture**, **Remove Last Measurement**, кэш 3D SAR и
|
||||
фильтр приближения GPR используют точный ключ (`python_app/models/collection_identity.py`),
|
||||
без эвристик по близости времён.
|
||||
|
||||
Результаты штатно приходят не по порядку: процессор пересчитывает сохранённые
|
||||
коллекции при смене настроек, а остановленное окно в режиме B-scan отправляет свои
|
||||
коллекции обратно в `preprocessed`. Поэтому:
|
||||
|
||||
- коллекция не новее последней принятой обрабатывается и публикуется, но не
|
||||
попадает в историю процессора и не уходит локатору;
|
||||
- при новой ревизии процессор пересчитывает новейшую коллекцию, в B-scan — всю
|
||||
историю; локатору уходит только новейшая;
|
||||
- на время такого пересчёта окно пишет `reprocess_current_result: false`, чтобы
|
||||
процессор не пересчитывал параллельно;
|
||||
- история окна вставляет результат по времени съёма; на диск пишутся только новые
|
||||
коллекции;
|
||||
- удалённые коллекции (`history_clear_before_ns`, `history_removed`) отбрасываются и
|
||||
процессором, и окном, даже если их результаты придут позже.
|
||||
|
||||
## Окно программы
|
||||
|
||||
`gui/app_window.py` только создаёт контроллеры, связывает их и закрывает окно.
|
||||
Раскладку строит `gui/window_view.py`; секции `gui/controllers/sections/` — наборы
|
||||
виджетов, сигналы подключают контроллеры. Миксинов и фасадов нет.
|
||||
|
||||
| Контроллер (`gui/controllers/`) | Отвечает за |
|
||||
| --- | --- |
|
||||
| `profile_controller` | профиль: выбор при запуске, загрузка, сохранение, профиль рядом с данными |
|
||||
| `config_controller` | перевод полей окна в модели и обратно по метаданным; состояния не хранит |
|
||||
| `radar_settings_controller` | развёртка, АЦП и лазер, пары сбора, пределы прибора |
|
||||
| `live_settings_controller` | режим обработки, ревизия, live-файл, пакет настроек из веба |
|
||||
| `pipeline_controller` | процессы, кольца, опрос и восстановление, **Single Capture**, приостановка, пересчёт B-scan |
|
||||
| `history_controller` | истории стадий, удаления |
|
||||
| `plot_controller` и `renderers/` | графики, выбор пары B-scan, проверка ревизии, счётчик изменений картинки для веба |
|
||||
| `snapshot_controller`, `recording_controller` | сохранение данных, очистка и удаление, запись на диск |
|
||||
| `preprocess_controller` | наборы, окно предобработки, съём, нейтральные наборы, временный референс |
|
||||
| `object_calibration_controller`, `object_calibration_capture` | калибровка по объекту |
|
||||
| `web_controller` | веб-сервер, очередь действий, снимки состояния и картинки |
|
||||
| `control_button_controller`, `log_controller` | GPIO-кнопка; журнал и окна ошибок |
|
||||
|
||||
Поля настроек описаны один раз — метаданными dataclass-моделей в
|
||||
`python_app/models/gui_profile_schema.py` (их разворачивает `setting_fields.py`).
|
||||
Из этого описания берутся ключ профиля, виджет, ключ live-файла, поле веб-формы и
|
||||
проверка при чтении. Подписи веб-формы берутся из подписей полей окна. Qt в моделях и
|
||||
кодеках нет. Оформление окна — `gui/theme.py`, цвета серий и маркеров —
|
||||
`gui/controllers/renderers/common.py`, оформление веба — `webui/static/style.css`.
|
||||
|
||||
### Правила жизненного цикла
|
||||
|
||||
- Приостановка (`gui/runtime/pipeline_pause.py`). Конвейер берёт владение до
|
||||
остановки и дочитывания колец, потому что дочитывание прокачивает события Qt.
|
||||
Асинхронный съём держит приостановку до конца. Автоперезапуск при удержанной
|
||||
приостановке запрещён; продолжается только то, что шло до неё.
|
||||
- **Stop** и закрытие окна. Пользовательский **Stop** сразу увеличивает счётчик остановок
|
||||
и отменяет ожидающие продолжения; техническая остановка приостановку не отменяет.
|
||||
Закрытие сразу запрещает новые запуски; если идёт дочитывание, его владелец
|
||||
заканчивает и вызывает отложенное закрытие.
|
||||
- **Start** проверяет всё до того, как что-то остановить. Отказ не трогает работающий
|
||||
процессор, локатор и историю.
|
||||
- Ошибка отрисовки не останавливает сбор: она пишется один раз и видна в строке
|
||||
состояния. Сбор останавливают только ошибки чтения колец.
|
||||
- Показывается новейшая коллекция: опрос рисует последний результат истории, даже
|
||||
если последним пришёл пересчёт более старой.
|
||||
- Графики меняются только через `PlotController`. Он считает изменения картинки, и
|
||||
веб снимает кадр по этому счётчику. Сигнал `QGraphicsScene.changed` не подключать:
|
||||
подписка переводит Qt в старый режим обновления, при котором на экране остаются
|
||||
следы прежних кривых (это проверяет тест
|
||||
`test_redrawn_views_leave_no_stale_pixels_on_screen`).
|
||||
- Без экрана и при нажатии GPIO-кнопки окна ошибок не показываются; приостановка
|
||||
освобождается до показа ошибки.
|
||||
- Одно действие — один код для кнопки окна, веба и GPIO, с общей проверкой
|
||||
занятости. Веб-поток трогает Qt только через сигналы с очередью и снимки состояния.
|
||||
- Синхронный съём (калибровка, референс, нейтральные наборы) идёт в потоке окна
|
||||
при приостановленном сборе; лишняя развёртка внутри такого нажатия — это зависшее
|
||||
окно. Съём для калибровки по объекту асинхронный, через обычный конвейер, а расчёт
|
||||
— в рабочем потоке без Qt.
|
||||
|
||||
## Состав кода
|
||||
|
||||
| Каталог | Что |
|
||||
| --- | --- |
|
||||
| `python_app/gui/` | окно, диалоги, контроллеры, отрисовка |
|
||||
| `python_app/hardware_full/` | сервисы приборов: `librevna_service.py`, `multi_device_service.py`, `matrix_radar_service.py`, `switched_matrix_radar_service.py`, `point_target_mock_service.py` (симулятор), `switch_service.py` |
|
||||
| `python_app/hardware_full/librevna_driver/` | USB-драйвер одиночного LibreVNA, протокол версии 14 |
|
||||
| `python_app/hardware_full/librevna_multi_device_driver/` | синхронная многоприборная развёртка (`cycle_collection.py` — один цикл) |
|
||||
| `python_app/hardware_full/rph_radar/`, `laser_control/` | радиофотонный радар: коллектор и плата RFG, плата лазеров |
|
||||
| `python_app/hardware_full/switch_drivers/` | ВЧ-ключи H7992 через GPIO (`gpio_uapi.py` — ioctl ядра без внешних пакетов) |
|
||||
| `python_app/models/` | профиль: схема, чтение и запись, проверки; модель данных (`dataset_model.py`) |
|
||||
| `python_app/orchestration/` | запуск процессов (`process_supervisor.py`, `restart_policy.py`), рабочие файлы, кольца (`shm/`) |
|
||||
| `python_app/storage/npz/` | наборы и наборы данных (`store.py`, `serialize.py`, `snapshot_numpy.py`, `vna_history_json.py`) |
|
||||
| `python_app/webui/` | веб-сервер FastAPI в процессе окна, страница `static/` |
|
||||
| `python_app/workflows/` | сценарии съёма и калибровки по объекту |
|
||||
| `python_app/scripts/` | процессы сбора и утилиты |
|
||||
| `python_app/tests/` | тесты Python |
|
||||
| `data_acq_and_processing/common_cpp/` | общий C++: конфигурация (`config/`), кадры и кольца (`ipc/`), драйвер H7992 (`hardware/`) |
|
||||
| `data_acq_and_processing/preprocessing/` | препроцессор и загрузка наборов |
|
||||
| `data_acq_and_processing/processing/` | процессор, режимы (`processors/src/`), локатор (`locator/`) |
|
||||
| `data_acq_and_processing/rph_radar_collector/` | коллектор L-Card E-502 |
|
||||
| `data_acq_and_processing/tests/` | тесты C++ и эталоны 3D SAR и GPR |
|
||||
| `data_acq_and_processing/third_party/` | nlohmann/json и Eigen |
|
||||
| `deploy/` | служба и обновление с флешки |
|
||||
| `device_firmware/` | прошивки и описания протоколов |
|
||||
|
||||
## Где что править
|
||||
|
||||
| Задача | Куда смотреть |
|
||||
| --- | --- |
|
||||
| ключи профиля и их проверка | `python_app/models/run_config_schema.py`, `run_config_codec.py`, `run_config_validation.py`; то же на стороне C++ — `data_acq_and_processing/common_cpp/config/` |
|
||||
| поля окна, live-файла и веб-формы | `python_app/models/gui_profile_schema.py`, секции `gui/controllers/sections/`, C++ — `processing/data_processor/src/processing_live_config.cpp` |
|
||||
| формат кадров | синхронно: C++ `common_cpp/ipc/` (`shared_types.hpp/.cpp`), Python-писатель `storage/npz/serialize.py`, Python-читатель `orchestration/shm/decoder.py`, описание — §6 `librevna_protocol.md` |
|
||||
| модель данных | `python_app/models/dataset_model.py` |
|
||||
| драйверы приборов | `python_app/hardware_full/` |
|
||||
| запуск процессов | `orchestration/process_supervisor.py`, `restart_policy.py`; из окна — `pipeline_controller.py` |
|
||||
| хранение данных | `python_app/storage/npz/` |
|
||||
| режимы обработки | `data_acq_and_processing/processing/processors/src/`: `bscan`, `gpr`, `passthrough`, `sar3d`; общий код GPR и 3D SAR — `backprojection.hpp`; отрисовка — `gui/controllers/renderers/` |
|
||||
| 3D SAR | `sar3d_processor.cpp` (алгоритм), `sar3d_clean.{hpp,cpp}` (фон кадра, CLEAN, рамки), `sar3d_gpu.{hpp,cpp}` (CUDA), `renderers/sar3d.py` |
|
||||
| сценарии съёма | `gui/controllers/preprocess_controller.py`, `python_app/workflows/` |
|
||||
| веб-интерфейс | `python_app/webui/`, мост к окну — `gui/controllers/web_controller.py` |
|
||||
|
||||
Изменив поведение, поправьте главу документации, где оно описано: ключи профиля —
|
||||
[configuration.md](configuration.md), поля окна — [desktop.md](desktop.md) и
|
||||
[processing.md](processing.md), форматы данных — [data.md](data.md), кадры колец и
|
||||
USB-протокол — `librevna_protocol.md`, переключение стимула —
|
||||
`per_point_switching.md`.
|
||||
|
||||
## 3D SAR и эталон
|
||||
|
||||
Алгоритм 3D SAR — порт исследовательского ноутбука `Multi_depthnorm_clean.ipynb`
|
||||
(ячейки 1 и 5; хранится вне репозитория). Комментарии в `sar3d_processor.cpp` и
|
||||
`sar3d_clean.cpp` называют функции и константы ноутбука, поля окна — ровно его имена.
|
||||
Величины ноутбука, не вынесенные в окно, — константы в начале этих файлов.
|
||||
|
||||
Решения CLEAN и рамок зависят от точных равенств (углы сетки лежат на границе окна
|
||||
дальности, полуразмеры окон кратны шагам), поэтому граница на видеокарте считается в
|
||||
double без FMA (`__dmul_rn`, `__dadd_rn`), а хост-код 3D SAR собирается с
|
||||
`-ffp-contract=off` (`SAR3D_HOST_FLAGS` в `Makefile`). Иначе на машинах с FMA (aarch64,
|
||||
x86 с `-mfma`) результат расходился бы с ноутбуком.
|
||||
|
||||
Сверка с ноутбуком автоматизирована (глава [Разработка](development.md)): генератор
|
||||
выполняет неизменённые функции ноутбука и пишет эталон `sar3d_reference.npz`, тест
|
||||
гоняет настоящий CUDA-бэкенд и сравнивает с ним. Без видеокарты проверяются только
|
||||
предупреждение и правила рамок, поэтому после правки 3D SAR сверку нужно прогнать на
|
||||
настоящей карте. GPR сверяется с `gpr_reference.npz` на CPU, без видеокарты.
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user