Compare commits
9
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
5826c4ff67 | ||
|
|
09a2aa7932 | ||
|
|
75185c7e36 | ||
|
|
5b70a9a4ce | ||
|
|
7334f89491 | ||
|
|
50f07caa64 | ||
|
|
8704db2de7 | ||
|
|
bd9395fc3d | ||
|
|
6f1eaf19e4 |
+5
-3
@@ -3,6 +3,8 @@ __pycache__/
|
||||
*.py[codz]
|
||||
*$py.class
|
||||
build*
|
||||
# build* иначе прячет скрипт сборки вендоренного gpr_cpp (его build/ остаётся игнорируемым)
|
||||
!data_acq_and_processing/gpr_cpp/build_cmake.sh
|
||||
.codex
|
||||
.DS_Store
|
||||
# C extensions
|
||||
@@ -245,13 +247,13 @@ __marimo__/
|
||||
python_app/runtime
|
||||
SHARE_INTERNET_TO_PI.md
|
||||
|
||||
# Локальная обвязка Claude Code и Codex: инструкции, хуки, скилы, конфиги инструментов.
|
||||
# В репозиторий не идёт. AGENTS.md — симлинк на CLAUDE.md для Codex.
|
||||
# Локальная обвязка Claude Code: инструкции, хуки, скилы, конфиги инструментов.
|
||||
# В репозиторий не идёт.
|
||||
CLAUDE.md
|
||||
AGENTS.md
|
||||
.claude/
|
||||
.graphifyignore
|
||||
pyrightconfig.json
|
||||
/docs/
|
||||
test_end_2/
|
||||
|
||||
# --- device_firmware: PlatformIO / STM32G431 cart remote ---
|
||||
|
||||
Vendored
+1
-1
@@ -1,3 +1,3 @@
|
||||
{
|
||||
"cmake.sourceDirectory": "/home/ayzen/Documents/radar_system/data_acq_and_processing/gpr_cpp"
|
||||
"cmake.sourceDirectory": "/home/awe/Documents/radar_system/data_acq_and_processing/gpr_cpp"
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,39 +1,60 @@
|
||||
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))
|
||||
# --- gpr_cpp: vendored C++/CUDA 3D SAR library (originally from VNA_AlGo).
|
||||
# Built automatically by the $(GPR_CPP_LIB) rule below via its own CMake
|
||||
# (build_cmake.sh). CUDA headers and libnvrtc come from the pip nvidia-*
|
||||
# packages inside this repo's venv (same detection as build_cmake.sh); when
|
||||
# those are absent (e.g. Jetson, where CUDA comes with JetPack) the system
|
||||
# toolkit at /usr/local/cuda is used. libcuda is always the system driver.
|
||||
GPR_CPP_DIR ?= data_acq_and_processing/gpr_cpp
|
||||
GPR_CPP_LIB ?= $(GPR_CPP_DIR)/build/libgpr_core_cpp.a
|
||||
# Absolute so the derived -rpath (libnvrtc dir) survives running the binary
|
||||
# from outside the repo root.
|
||||
GPR_VENV ?= $(CURDIR)/.venv
|
||||
GPR_CUDA_INC_DIR ?= $(dir $(firstword $(wildcard $(GPR_VENV)/lib/python*/site-packages/nvidia/*/include/nvrtc.h)))
|
||||
# cuda.h can live in a DIFFERENT pip package than nvrtc.h (Jetson: cuda_runtime
|
||||
# vs cuda_nvrtc); the unified cu13 package on dev machines has both in one dir.
|
||||
GPR_CUDA_RT_INC_DIR ?= $(dir $(firstword $(wildcard $(GPR_VENV)/lib/python*/site-packages/nvidia/*/include/cuda.h)))
|
||||
GPR_NVRTC_LIB ?= $(firstword $(wildcard $(GPR_VENV)/lib/python*/site-packages/nvidia/*/lib/libnvrtc.so*))
|
||||
# System-CUDA fallback. An empty GPR_CUDA_INC_DIR would also corrupt the compile
|
||||
# command: a bare `-I` swallows the next argument.
|
||||
ifeq ($(strip $(GPR_CUDA_INC_DIR)),)
|
||||
GPR_CUDA_INC_DIR := /usr/local/cuda/include
|
||||
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))
|
||||
ifeq ($(strip $(GPR_CUDA_RT_INC_DIR)),)
|
||||
GPR_CUDA_RT_INC_DIR := /usr/local/cuda/include
|
||||
endif
|
||||
ifeq ($(strip $(GPR_NVRTC_LIB)),)
|
||||
GPR_NVRTC_LIB := $(firstword $(wildcard /usr/local/cuda/lib64/libnvrtc.so*))
|
||||
endif
|
||||
# Deduplicated CUDA include flags (a single dir on dev machines).
|
||||
GPR_CUDA_INC_FLAGS := $(addprefix -I,$(sort $(GPR_CUDA_INC_DIR) $(GPR_CUDA_RT_INC_DIR)))
|
||||
GPR_CUDA_LIB_DIR := $(dir $(GPR_NVRTC_LIB))
|
||||
# libnvrtc from pip ships only as libnvrtc.so.N (no .so symlink), so link the
|
||||
# full path; the rpath makes the binary find it without LD_LIBRARY_PATH.
|
||||
DATA_PROCESSOR_LDFLAGS := $(LDFLAGS) $(GPR_CPP_LIB) $(GPR_NVRTC_LIB) -lcuda -Wl,-rpath,$(GPR_CUDA_LIB_DIR)
|
||||
|
||||
INCLUDES := \
|
||||
-I$(GPR_CPP_DIR)/include \
|
||||
$(GPR_CUDA_INC_FLAGS) \
|
||||
-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/preprocessing/bundles/include \
|
||||
-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/data_preprocessor/include \
|
||||
-Idata_acq_and_processing/processing/processors/include \
|
||||
-Idata_acq_and_processing/processing/data_processor/include \
|
||||
@@ -44,8 +65,21 @@ 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/bundles/src/channel_bundle_support.cpp \
|
||||
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/data_preprocessor/src/data_preprocessor.cpp \
|
||||
data_acq_and_processing/preprocessing/data_preprocessor/src/main.cpp
|
||||
|
||||
@@ -58,140 +92,113 @@ PROCESSOR_SOURCES := \
|
||||
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)
|
||||
|
||||
# RadioPhotonic radar (RPH) collector: application-owned L-Card E-502 acquisition binary. It loads the
|
||||
# Kamil ADC collector: standalone 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 == 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 \
|
||||
# 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 \
|
||||
-Idata_acq_and_processing/common_cpp/ipc/include \
|
||||
-Idata_acq_and_processing/common_cpp/config/include \
|
||||
-Idata_acq_and_processing/common_cpp/hardware/include \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/interfaces \
|
||||
-Idata_acq_and_processing/sweep_orchestrator/device_drivers/switches \
|
||||
-Idata_acq_and_processing/processing/locator/include \
|
||||
-Idata_acq_and_processing/third_party
|
||||
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
|
||||
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
|
||||
|
||||
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))
|
||||
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
|
||||
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))
|
||||
|
||||
TARGETS := \
|
||||
$(BIN_DIR)/sweep_orchestrator \
|
||||
$(BIN_DIR)/data_preprocessor \
|
||||
$(BIN_DIR)/data_processor
|
||||
|
||||
.PHONY: all clean rph_radar_collector check-native check-sar3d check-gpr
|
||||
.PHONY: all clean kamil_adc_collector sar3d_parity
|
||||
|
||||
all: $(TARGETS)
|
||||
|
||||
rph_radar_collector: $(BIN_DIR)/rph_radar_collector
|
||||
kamil_adc_collector: $(BIN_DIR)/kamil_adc_collector
|
||||
|
||||
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
|
||||
# On-demand parity check of the sar3d processor against the reference notebook
|
||||
# volumes (scripts/sar3d_volume_compare.py diffs the dumped volume against a
|
||||
# *_volume.npz export). Not part of `all`.
|
||||
SAR3D_PARITY_OBJS := $(addprefix $(BUILD_DIR)/, \
|
||||
data_acq_and_processing/processing/tools/sar3d_parity_main.o \
|
||||
data_acq_and_processing/processing/processors/src/sar3d_processor.o \
|
||||
data_acq_and_processing/common_cpp/config/src/run_config.o \
|
||||
data_acq_and_processing/common_cpp/ipc/src/shared_types.o)
|
||||
|
||||
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)
|
||||
sar3d_parity: $(BIN_DIR)/sar3d_parity
|
||||
|
||||
# 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
|
||||
$(BIN_DIR)/sar3d_parity: $(SAR3D_PARITY_OBJS) $(GPR_CPP_LIB)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(SAR3D_PARITY_OBJS) -o $@ $(DATA_PROCESSOR_LDFLAGS)
|
||||
|
||||
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)/sweep_orchestrator: $(SWEEP_ORCH_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(SWEEP_ORCH_OBJS) -o $@ $(ORCH_LDFLAGS)
|
||||
|
||||
$(BIN_DIR)/data_preprocessor: $(PREPROCESSOR_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(PREPROCESSOR_OBJS) -o $@ $(LDFLAGS)
|
||||
|
||||
$(BIN_DIR)/data_processor: $(DATA_PROCESSOR_OBJS) $(SAR3D_CUDA_STAMP)
|
||||
$(BIN_DIR)/data_processor: $(DATA_PROCESSOR_OBJS) $(GPR_CPP_LIB)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(CXX) $(DATA_PROCESSOR_OBJS) -o $@ $(LDFLAGS) $(SAR3D_LDFLAGS)
|
||||
$(if $(SAR3D_LDFLAGS),,@echo "note: CUDA not found, selecting sar3d will show a warning")
|
||||
$(CXX) $(DATA_PROCESSOR_OBJS) -o $@ $(DATA_PROCESSOR_LDFLAGS)
|
||||
|
||||
$(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)
|
||||
# Vendored gpr_cpp static lib: delegate to its CMake build (also produces the
|
||||
# standalone_bp example used by sar3d_parity cross-checks).
|
||||
GPR_CPP_SRC_FILES := $(wildcard $(GPR_CPP_DIR)/src/*) $(wildcard $(GPR_CPP_DIR)/include/gpr/*) \
|
||||
$(wildcard $(GPR_CPP_DIR)/examples/*) $(GPR_CPP_DIR)/CMakeLists.txt $(GPR_CPP_DIR)/build_cmake.sh
|
||||
|
||||
# Keep source-relative paths so moving a source also replaces its dependencies.
|
||||
$(RPH_BUILD_DIR)/%.o: %.cpp
|
||||
$(GPR_CPP_LIB): $(GPR_CPP_SRC_FILES)
|
||||
GPR_BUILD_PYTHON=OFF VENV=$(abspath $(GPR_VENV)) bash $(GPR_CPP_DIR)/build_cmake.sh
|
||||
|
||||
# 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)))
|
||||
|
||||
$(KAMIL_BUILD_DIR)/%.o: %.cpp
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXXFLAGS) $(RPH_COLLECTOR_INCLUDES) -c $< -o $@
|
||||
$(CXX) $(CXXFLAGS) $(KAMIL_COLLECTOR_INCLUDES) -c $< -o $@
|
||||
|
||||
$(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))
|
||||
$(BIN_DIR)/kamil_adc_collector: $(KAMIL_COLLECTOR_OBJS)
|
||||
@mkdir -p $(BIN_DIR)
|
||||
$(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)
|
||||
$(CXX) $(KAMIL_COLLECTOR_OBJS) -o $@ $(KAMIL_COLLECTOR_LDFLAGS)
|
||||
|
||||
$(BUILD_DIR)/%.o: %.cpp
|
||||
@mkdir -p $(dir $@)
|
||||
$(CXX) $(CXXFLAGS) $(INCLUDES) -c $< -o $@
|
||||
$(CXX) $(CXXFLAGS) $(VISA_CXXFLAGS) $(INCLUDES) -c $< -o $@
|
||||
|
||||
-include $(DEPFILES)
|
||||
|
||||
clean:
|
||||
rm -rf $(BUILD_DIR)
|
||||
rm -rf $(BUILD_DIR) $(GPR_CPP_DIR)/build
|
||||
|
||||
@@ -1,84 +1,209 @@
|
||||
# Radar System
|
||||
|
||||
Программа радиолокационной системы. Она управляет радаром, снимает развёртки
|
||||
коэффициента передачи S21 по всем парам антенн, калибрует их, строит изображения сцены
|
||||
и показывает их в окне программы и в браузере. Найденные объекты она отдаёт внешним
|
||||
программам по TCP. Программа работает на рабочей станции с экраном и на Raspberry Pi 5
|
||||
без оператора.
|
||||
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.
|
||||
|
||||
Радары, с которыми она работает:
|
||||
## 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 |
|
||||
| --- | --- |
|
||||
| одиночный LibreVNA | один векторный анализатор цепей и ВЧ-ключи на антеннах |
|
||||
| многоприборная схема | три синхронных LibreVNA; с контроллером ключей — матрица 4 × 4 пар за одну развёртку |
|
||||
| радиофотонный радар | СВЧ-сигнал формируют лазеры, данные оцифровывает АЦП L-Card E-502 или плата RFG |
|
||||
| симулятор | работа без железа: один движущийся точечный отражатель |
|
||||
| `data_acq_and_processing/` | C++ pipeline: `sweep_orchestrator/`, `preprocessing/`, `processing/` (including `locator/`), shared `common_cpp/` (config, IPC rings), the vendored `gpr_cpp/` C++/CUDA 3D SAR library, 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. |
|
||||
|
||||
Режимы обработки: pass-through (S21 по частоте), B-scan (дальность во времени), GPR
|
||||
(карта земли перед радаром с поиском объектов), 3D SAR (трёхмерное изображение на
|
||||
видеокарте NVIDIA).
|
||||
## Requirements
|
||||
|
||||
## Быстрый старт без железа
|
||||
- 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.
|
||||
- For the `sar3d` processor (3D SAR): an NVIDIA GPU with the driver installed
|
||||
(`libcuda`) and `cmake` (the vendored `gpr_cpp` library at
|
||||
`data_acq_and_processing/gpr_cpp/` builds via its own CMake, invoked
|
||||
automatically by `make`). CUDA headers/libnvrtc are taken from the pip
|
||||
`nvidia-*` packages in this repo's venv (installed by `requirements.txt` on
|
||||
x86_64), otherwise from the system toolkit at `/usr/local/cuda`
|
||||
(Jetson/JetPack, apt `cuda-toolkit`).
|
||||
|
||||
```sh
|
||||
## 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/
|
||||
```
|
||||
|
||||
`make` also builds the vendored 3D SAR library
|
||||
(`data_acq_and_processing/gpr_cpp/`, CMake + NVRTC — needs `cmake`; CUDA
|
||||
headers come from the pip `nvidia-*` packages in `.venv`, or `/usr/local/cuda`
|
||||
as a fallback).
|
||||
|
||||
To verify the sar3d processor against the gpr_cpp reference on the recorded
|
||||
dataset, build and run the on-demand parity tool:
|
||||
|
||||
```bash
|
||||
make sar3d_parity
|
||||
./build/bin/sar3d_parity <preprocessed_dataset_dir> <center_snapshot_name>
|
||||
# compare the printed peak/argmax with
|
||||
# ./data_acq_and_processing/gpr_cpp/build/standalone_bp on the same arguments
|
||||
```
|
||||
|
||||
Run the desktop GUI:
|
||||
|
||||
```bash
|
||||
.venv/bin/python -m python_app.gui.main
|
||||
# or, with the build step and single-instance handling:
|
||||
./start.sh
|
||||
```
|
||||
|
||||
`start.sh` создаёт окружение Python, ставит пакеты и собирает C++. Корневой профиль
|
||||
`run_config.json` — симулятор. В окне нажмите **Preprocessing…** →
|
||||
**Create Neutral S21 Sets**, затем **Start** и выберите режим в списке **Mode**.
|
||||
`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.
|
||||
|
||||
Установка, запуск с железом и служба для работы без экрана — в главе
|
||||
[Установка и запуск](docs/installation.md).
|
||||
## Jetson (aarch64) bring-up
|
||||
|
||||
## Документация
|
||||
Ubuntu 22.04 on Jetson has glibc 2.35, and PyQt6 6.8+ ships only
|
||||
`manylinux_2_39` aarch64 wheels — `requirements.txt` therefore pins PyQt6 to
|
||||
`<6.8` on aarch64 (6.7.1 is the last release with a compatible wheel). With
|
||||
that pin the stock `./start.sh` provisioning works; the extra steps are only
|
||||
for the CUDA-backed `sar3d` processor:
|
||||
|
||||
Для работы с радаром:
|
||||
```bash
|
||||
# 1. copy the replay dataset (for dataset-replay profiles) and point
|
||||
# radar.replay.dataset_dir in the profile at the local copy — the path in
|
||||
# run_config_examples/*.json is machine-specific.
|
||||
|
||||
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).
|
||||
# 2. normal launch; the vendored gpr_cpp builds against the JetPack system CUDA
|
||||
# (requirements.txt installs no pip nvidia-* packages on aarch64, and
|
||||
# build_cmake.sh falls back to /usr/local/cuda automatically; override with
|
||||
# CUDA_INC=... CUDA_LIB=... if the toolkit lives elsewhere):
|
||||
./start.sh --profile run_config_examples/run_config_replay_vna.example.json
|
||||
```
|
||||
|
||||
Для разработки:
|
||||
If linking fails with `cannot find -lcuda`, the driver stub lives at
|
||||
`/usr/local/cuda/lib64/stubs/libcuda.so`; on JetPack the real driver is
|
||||
`/usr/lib/aarch64-linux-gnu/nvidia/libcuda.so.1` (present after boot).
|
||||
|
||||
- [Устройство программы](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).
|
||||
## Running without hardware (emulation)
|
||||
|
||||
## Состав репозитория
|
||||
Two ways to run the full pipeline with no radar attached:
|
||||
|
||||
| Каталог | Что |
|
||||
| --- | --- |
|
||||
| `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-пакеты |
|
||||
**Mock producer (synthetic data).** The default `run_config.json` already sets
|
||||
`radar.driver_mode: "mock"`, so the producer generates synthetic S-parameters.
|
||||
The preprocessor still requires S21 calibration/reference sets; create them once:
|
||||
|
||||
```bash
|
||||
.venv/bin/python -m python_app.scripts.manual_smoke_run --duration 1.0 # creates smoke_cal/smoke_ref
|
||||
./start.sh # then in the GUI: Preprocessing Panel -> pick the sets -> Start
|
||||
```
|
||||
|
||||
(Alternatively use the GUI's "Create neutral sets" button, which makes
|
||||
identity calibration + zero reference so preprocessing is a pass-through.)
|
||||
|
||||
**Dataset replay (recorded scene, meaningful sar3d picture).** When
|
||||
`radar.replay.dataset_dir` is set in the profile, the supervisor launches
|
||||
`python_app.scripts.replay_raw_producer` instead of the normal producer: it
|
||||
replays recorded snapshots into the raw ring at the recorded cadence, looping
|
||||
forever with a monotonic timeline. Pair it with NEUTRAL preprocess sets so the
|
||||
already-preprocessed recording passes through the preprocessor bit-exactly.
|
||||
A ready profile for the VNA_AlGo tank005 dataset is provided:
|
||||
|
||||
```bash
|
||||
# one-time: neutral sets for this profile (or use the GUI button)
|
||||
.venv/bin/python -m python_app.scripts.create_neutral_preprocess_sets \
|
||||
--config run_config_examples/run_config_replay_vna.example.json
|
||||
# interactive GUI (mode sar3d is pre-selected by the profile's gui section):
|
||||
./start.sh --profile run_config_examples/run_config_replay_vna.example.json
|
||||
# or a fully headless end-to-end check:
|
||||
.venv/bin/python -m python_app.scripts.replay_smoke_run --duration 30
|
||||
```
|
||||
|
||||
Note: the sar3d processor allocates GPU memory; if the desktop (e.g. a browser)
|
||||
has filled the GPU, sar3d logs `out of memory` and publishes empty results until
|
||||
memory frees. Reduce `sar3d_grid_nx/ny/nz` in the processing panel to shrink
|
||||
the footprint.
|
||||
|
||||
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.
|
||||
|
||||
## 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
|
||||
|
||||
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
|
||||
|
||||
```bash
|
||||
.venv/bin/python -m unittest discover -s python_app/tests
|
||||
```
|
||||
|
||||
@@ -1,53 +0,0 @@
|
||||
#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,15 +14,43 @@ 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 {
|
||||
@@ -34,25 +62,39 @@ 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 live processing configuration.
|
||||
// Runtime pacing and behavior flags for C++ workers.
|
||||
std::uint32_t settling_ms = 0;
|
||||
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{};
|
||||
PreprocessAssetConfig object_calibration{};
|
||||
};
|
||||
|
||||
struct S11CalibrationPreprocessConfig {
|
||||
PreprocessAssetConfig open{};
|
||||
PreprocessAssetConfig short_standard{};
|
||||
PreprocessAssetConfig load{};
|
||||
};
|
||||
|
||||
struct S11PreprocessConfig {
|
||||
S11CalibrationPreprocessConfig calibration{};
|
||||
PreprocessAssetConfig reference{};
|
||||
};
|
||||
|
||||
struct PreprocessNotchBandConfig {
|
||||
@@ -70,6 +112,7 @@ struct PreprocessNotchConfig {
|
||||
struct PreprocessConfig {
|
||||
// Channel-specific preprocessing assets selected by Python GUI layer.
|
||||
S21PreprocessConfig s21{};
|
||||
S11PreprocessConfig s11{};
|
||||
PreprocessNotchConfig notch{};
|
||||
};
|
||||
|
||||
@@ -91,14 +134,15 @@ struct GprRxGeometry {
|
||||
};
|
||||
|
||||
struct GprConfig {
|
||||
// Initial GPR settings; the data_processor can override them through live config.
|
||||
// Stable collection-level GPR processing settings.
|
||||
float relative_permittivity = 1.0F;
|
||||
std::vector<GprTxGeometry> tx_geometry{};
|
||||
std::vector<GprRxGeometry> rx_geometry{};
|
||||
};
|
||||
|
||||
struct RunConfig {
|
||||
// Single configuration object shared by all C++ processes.
|
||||
std::string radar_model = "librevna";
|
||||
RadarConfig radar{};
|
||||
SwitchConfig input_switch{};
|
||||
SwitchConfig output_switch{};
|
||||
RingsConfig rings{};
|
||||
@@ -110,6 +154,5 @@ 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
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
#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,18 +65,7 @@ using Json = nlohmann::json;
|
||||
if (!value.is_number()) {
|
||||
throw std::runtime_error("Expected JSON number at " + context);
|
||||
}
|
||||
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);
|
||||
return value.get<double>();
|
||||
}
|
||||
|
||||
[[nodiscard]] auto number_to_u32(double value, const std::string& context) -> std::uint32_t {
|
||||
@@ -85,7 +74,7 @@ using Json = nlohmann::json;
|
||||
}
|
||||
|
||||
const auto rounded = std::round(value);
|
||||
if (value != rounded) {
|
||||
if (std::fabs(value - rounded) > 0.000001) {
|
||||
throw std::runtime_error("Expected integer value at " + context);
|
||||
}
|
||||
return static_cast<std::uint32_t>(rounded);
|
||||
@@ -93,7 +82,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 (value != rounded) {
|
||||
if (std::fabs(value - rounded) > 0.000001) {
|
||||
throw std::runtime_error("Expected integer value at " + context);
|
||||
}
|
||||
|
||||
@@ -146,7 +135,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 number_to_f32(as_number(*value, key), key);
|
||||
return static_cast<float>(as_number(*value, key));
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
@@ -160,6 +149,7 @@ 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;
|
||||
}
|
||||
@@ -183,8 +173,8 @@ using Json = nlohmann::json;
|
||||
}
|
||||
|
||||
PreprocessNotchBandConfig band{};
|
||||
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);
|
||||
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]"));
|
||||
notch.bands_hz.push_back(band);
|
||||
}
|
||||
}
|
||||
@@ -241,7 +231,30 @@ 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);
|
||||
}
|
||||
@@ -257,11 +270,21 @@ void validate_switch_config(const SwitchConfig& config) {
|
||||
throw std::runtime_error("gpio_chip is required for native switch mode: " + config.name);
|
||||
}
|
||||
|
||||
if (config.positions > 4U) {
|
||||
throw std::runtime_error("Native H7992 switch driver supports positions <= 4 for " + 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.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);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -274,9 +297,36 @@ 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");
|
||||
@@ -312,24 +362,22 @@ void validate_combo_key(const ipc::ComboKey& key, const RunConfig& config) {
|
||||
|
||||
[[nodiscard]] auto parse_switch_config(
|
||||
const Json& object,
|
||||
const std::string& default_name
|
||||
const std::string& default_name,
|
||||
std::uint32_t default_radar_port
|
||||
) -> 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"));
|
||||
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.driver_kind = parse_switch_driver_kind(optional_string(*switch_obj, "driver", "h7992"));
|
||||
config.radar_port = optional_u32(*switch_obj, "radar_port", default_radar_port);
|
||||
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);
|
||||
|
||||
@@ -358,12 +406,6 @@ void validate_combo_key(const ipc::ComboKey& key, const RunConfig& 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);
|
||||
}
|
||||
@@ -373,35 +415,6 @@ void validate_combo_key(const ipc::ComboKey& key, const RunConfig& 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()) {
|
||||
@@ -425,18 +438,68 @@ 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.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);
|
||||
}
|
||||
|
||||
{
|
||||
const auto* switches_obj = as_object(required_field(*root_obj, "switches"), "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");
|
||||
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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",
|
||||
@@ -477,10 +540,17 @@ 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");
|
||||
if (optional_field(*s21_obj, "object_calibration") != nullptr) {
|
||||
config.preprocess.s21.object_calibration =
|
||||
parse_preprocess_asset(*s21_obj, "object_calibration", "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 (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");
|
||||
@@ -496,6 +566,7 @@ 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,
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@@ -27,15 +26,16 @@ struct ComboKey {
|
||||
|
||||
struct ComboKeyHash {
|
||||
[[nodiscard]] auto operator()(const ComboKey& key) const noexcept -> std::size_t {
|
||||
const auto packed = (static_cast<std::uint64_t>(key.input_pos) << 32U) | key.output_pos;
|
||||
return std::hash<std::uint64_t>{}(packed);
|
||||
return (static_cast<std::size_t>(key.input_pos) << 32U) ^ static_cast<std::size_t>(key.output_pos);
|
||||
}
|
||||
};
|
||||
|
||||
struct SweepTraceBlock {
|
||||
ComboKey combo{};
|
||||
// Frequency axis in Hz. Must have the same size as `s21`.
|
||||
// Frequency axis in Hz. Must have the same size as `s11` and `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,12 +90,11 @@ struct ResultBlock {
|
||||
struct ResultCollection {
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t monotonic_ns = 0;
|
||||
// Processing duration and acquisition identity, independent of stage completion time.
|
||||
// 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.
|
||||
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{};
|
||||
};
|
||||
@@ -110,6 +109,7 @@ 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,11 +25,19 @@ 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 unread payload; false only when none remains.
|
||||
// Pops the next payload if available.
|
||||
[[nodiscard]] auto pop(std::vector<std::uint8_t>& payload) -> bool;
|
||||
|
||||
private:
|
||||
@@ -45,14 +53,11 @@ 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);
|
||||
[[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.
|
||||
// Validates that mapped_size covers the geometry declared in the header
|
||||
// (fix #50: reject undersized/corrupt mappings to prevent OOB slot access).
|
||||
static void validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name);
|
||||
};
|
||||
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
#include "shared_types.hpp"
|
||||
|
||||
#include <bit>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
@@ -13,11 +12,9 @@
|
||||
namespace radar::ipc {
|
||||
namespace {
|
||||
|
||||
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
|
||||
constexpr std::uint32_t kRawCollectionMagic = 0x32574152U; // RAW2
|
||||
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x32525050U; // PRP2
|
||||
constexpr std::uint32_t kResultCollectionMagic = 0x324C5352U; // RSL2
|
||||
|
||||
template <typename T>
|
||||
concept TriviallySerializable = std::is_trivially_copyable_v<T>;
|
||||
@@ -78,6 +75,16 @@ 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();
|
||||
}
|
||||
@@ -88,7 +95,7 @@ class BinaryReader {
|
||||
|
||||
private:
|
||||
void ensure_available(std::size_t size) const {
|
||||
if (size > bytes_.size() - offset_) {
|
||||
if (offset_ + size > bytes_.size()) {
|
||||
throw std::runtime_error("Unexpected end of serialized payload");
|
||||
}
|
||||
}
|
||||
@@ -99,6 +106,7 @@ 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);
|
||||
@@ -108,6 +116,11 @@ 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);
|
||||
@@ -119,8 +132,9 @@ 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::uint64_t count, std::size_t min_bytes_each, BinaryReader& reader) {
|
||||
if (min_bytes_each != 0U && count > reader.remaining_bytes() / min_bytes_each) {
|
||||
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()) {
|
||||
throw std::runtime_error("Declared element count exceeds remaining payload bytes");
|
||||
}
|
||||
}
|
||||
@@ -131,15 +145,23 @@ void require_count_fits(std::uint64_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) + s21 (8B) = 12 bytes.
|
||||
require_count_fits(point_count, 12U, reader);
|
||||
// Each point encodes frequency (4B) + s11 (8B) + s21 (8B) = 20 bytes.
|
||||
require_count_fits(point_count, 20U, 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>(),
|
||||
@@ -150,7 +172,10 @@ void require_count_fits(std::uint64_t count, std::size_t min_bytes_each, BinaryR
|
||||
return trace;
|
||||
}
|
||||
|
||||
// Capture trailer: collection window, then one per-trace window in trace order.
|
||||
// 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).
|
||||
void write_trace_collection(BinaryWriter& writer, std::uint32_t magic, const RawSweepCollection& collection) {
|
||||
writer.write(magic);
|
||||
writer.write(collection.collection_id);
|
||||
@@ -189,9 +214,22 @@ 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");
|
||||
@@ -217,6 +255,23 @@ 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");
|
||||
@@ -238,6 +293,25 @@ 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()) {
|
||||
@@ -257,6 +331,16 @@ 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);
|
||||
@@ -279,6 +363,29 @@ 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);
|
||||
@@ -289,6 +396,20 @@ 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);
|
||||
@@ -329,10 +450,6 @@ 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"));
|
||||
|
||||
@@ -346,6 +463,35 @@ 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,6 +1,5 @@
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cerrno>
|
||||
@@ -31,7 +30,7 @@ struct alignas(64) ShmRing::Header {
|
||||
std::atomic<std::uint64_t> dropped{};
|
||||
};
|
||||
|
||||
struct alignas(8) ShmRing::SlotHeader {
|
||||
struct alignas(16) ShmRing::SlotHeader {
|
||||
std::uint32_t payload_size = 0;
|
||||
std::uint32_t reserved = 0;
|
||||
std::uint64_t sequence = 0;
|
||||
@@ -43,12 +42,64 @@ 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);
|
||||
constexpr std::uint64_t kMaxMappedSize = 1ULL << 40U;
|
||||
|
||||
[[nodiscard]] auto checked_u64_diff(std::uint64_t left, std::uint64_t right) -> std::uint64_t {
|
||||
return left >= right ? left - right : 0;
|
||||
}
|
||||
|
||||
[[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() {
|
||||
@@ -77,7 +128,12 @@ auto ShmRing::open_or_create(
|
||||
std::uint32_t slot_size_bytes
|
||||
) -> ShmRing {
|
||||
validate_name(name);
|
||||
const auto mapped_size = checked_mapped_size(capacity, slot_size_bytes, 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");
|
||||
}
|
||||
|
||||
bool created = false;
|
||||
int fd = shm_open(name.c_str(), O_RDWR | O_CREAT | O_EXCL, 0660);
|
||||
@@ -91,30 +147,15 @@ auto ShmRing::open_or_create(
|
||||
throw errno_message("Failed to open shared memory ring", name);
|
||||
}
|
||||
|
||||
ShmRing ring{};
|
||||
ring.fd_ = fd;
|
||||
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;
|
||||
|
||||
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);
|
||||
@@ -122,29 +163,28 @@ auto ShmRing::open_or_create(
|
||||
throw errno_message("Failed to mmap shared memory ring", name);
|
||||
}
|
||||
|
||||
ring.mapped_ = mapped;
|
||||
ring.mapped_size_ = mapped_size;
|
||||
auto* header = ring.header_ = static_cast<Header*>(mapped);
|
||||
ScopedMmap scoped_mmap(mapped, mapped_size);
|
||||
auto* header = static_cast<Header*>(mapped);
|
||||
|
||||
if (created) {
|
||||
// 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::memset(mapped, 0, mapped_size);
|
||||
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);
|
||||
// Publish readiness only after geometry and cursors have been initialized.
|
||||
std::atomic_ref(header->version).store(kRingVersion, std::memory_order_release);
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
} else {
|
||||
const auto deadline = std::chrono::steady_clock::now() + kHeaderInitWaitTimeout;
|
||||
|
||||
while (true) {
|
||||
const bool version_ok = std::atomic_ref(header->version).load(std::memory_order_acquire) == kRingVersion;
|
||||
std::atomic_thread_fence(std::memory_order_acquire);
|
||||
|
||||
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) {
|
||||
@@ -173,13 +213,89 @@ 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");
|
||||
@@ -191,7 +307,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 (write_seq >= read_seq && write_seq - read_seq >= header_->capacity) {
|
||||
if (checked_u64_diff(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(
|
||||
@@ -203,13 +319,14 @@ auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
|
||||
}
|
||||
|
||||
auto* slot = slot_header(write_seq);
|
||||
// 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);
|
||||
// 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.)
|
||||
slot->payload_size = static_cast<std::uint32_t>(payload.size());
|
||||
std::memcpy(slot_payload(slot), payload.data(), payload.size());
|
||||
std::atomic_ref(slot->sequence).store(write_seq + 1U, std::memory_order_release);
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
slot->sequence = write_seq + 1U;
|
||||
|
||||
std::atomic_thread_fence(std::memory_order_release);
|
||||
header_->write_seq.store(write_seq + 1U, std::memory_order_release);
|
||||
@@ -221,49 +338,41 @@ auto ShmRing::pop(std::vector<std::uint8_t>& payload) -> bool {
|
||||
throw std::runtime_error("Ring is not open");
|
||||
}
|
||||
|
||||
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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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* 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;
|
||||
}
|
||||
|
||||
auto ShmRing::slot_stride_bytes() const -> std::size_t {
|
||||
@@ -296,41 +405,37 @@ void ShmRing::close() {
|
||||
}
|
||||
|
||||
void ShmRing::validate_name(const std::string& name) {
|
||||
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");
|
||||
if (name.empty() || name.front() != '/') {
|
||||
throw std::runtime_error("POSIX shm name must start with '/'");
|
||||
}
|
||||
}
|
||||
|
||||
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("Ring capacity must be > 0");
|
||||
}
|
||||
if (slot_size_bytes == 0U) {
|
||||
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");
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
return static_cast<std::size_t>(mapped_size_u64);
|
||||
}
|
||||
|
||||
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)) {
|
||||
// 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;
|
||||
if (capacity == 0U) {
|
||||
throw std::runtime_error("Shared memory ring capacity is zero for " + name);
|
||||
}
|
||||
if (slot_size_bytes == 0U) {
|
||||
throw std::runtime_error("Shared memory ring slot size is zero for " + name);
|
||||
}
|
||||
|
||||
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)) {
|
||||
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;
|
||||
|
||||
if (mapped_size < expected_size) {
|
||||
throw std::runtime_error("Shared memory ring size is too small for " + name);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
# 3.22 = apt cmake на Ubuntu 22.04 (Jetson/JetPack); фич новее здесь нет.
|
||||
cmake_minimum_required(VERSION 3.22)
|
||||
project(gpr_cpp LANGUAGES CXX) # CXX-only: CUDA компилируется в рантайме через NVRTC
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
|
||||
|
||||
# bindings/pymodule.cpp не вендорен в radar_system — включение потребует вернуть его из VNA_AlGo
|
||||
option(GPR_BUILD_PYTHON "Build pybind11 module" OFF)
|
||||
option(GPR_BUILD_TESTS "Build C++ parity tests" OFF)
|
||||
option(GPR_BUILD_EXAMPLES "Build standalone C++ example" ON)
|
||||
|
||||
# Python окружение (venv) — источник pybind11 и pip CUDA-библиотек (nvrtc, cudart).
|
||||
if(NOT DEFINED PYTHON_EXECUTABLE)
|
||||
set(PYTHON_EXECUTABLE python3)
|
||||
endif()
|
||||
|
||||
# CUDA include/lib из pip-пакетов nvidia-*; задаются -DGPR_CUDA_INC / -DGPR_CUDA_LIB
|
||||
# (детект делает build_cmake.sh). Заголовки: nvrtc.h, cuda.h (Driver API).
|
||||
# GPR_CUDA_INC может быть СПИСКОМ каталогов (через ';'): в раздельных pip-пакетах
|
||||
# (Jetson: cuda_nvrtc + cuda_runtime) nvrtc.h и cuda.h лежат в разных include/.
|
||||
if(NOT DEFINED GPR_CUDA_INC OR NOT DEFINED GPR_CUDA_LIB)
|
||||
message(FATAL_ERROR "Задайте -DGPR_CUDA_INC=<dir(ы) с nvrtc.h,cuda.h; ';'-список> и -DGPR_CUDA_LIB=<dir с libnvrtc.so*>")
|
||||
endif()
|
||||
|
||||
# libcuda (драйвер) — системный
|
||||
find_library(CUDA_DRIVER_LIB cuda PATHS /usr/lib/x86_64-linux-gnu /usr/lib/aarch64-linux-gnu
|
||||
/usr/lib REQUIRED)
|
||||
# libnvrtc из pip
|
||||
find_library(NVRTC_LIB NAMES nvrtc libnvrtc.so.13 libnvrtc.so.12 nvrtc.so
|
||||
PATHS ${GPR_CUDA_LIB} REQUIRED NO_DEFAULT_PATH)
|
||||
|
||||
# PocketFFT (header-only, vendored)
|
||||
add_library(pocketfft INTERFACE)
|
||||
target_include_directories(pocketfft INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/third_party/pocketfft)
|
||||
|
||||
# --- ядро библиотеки (usable без Python) ---
|
||||
add_library(gpr_core_cpp STATIC
|
||||
src/geometry.cpp
|
||||
src/ascan.cpp
|
||||
src/device.cpp
|
||||
src/nvrtc_kernel.cpp
|
||||
src/sar3d.cpp)
|
||||
target_include_directories(gpr_core_cpp PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${GPR_CUDA_INC})
|
||||
target_link_libraries(gpr_core_cpp PUBLIC pocketfft ${NVRTC_LIB} ${CUDA_DRIVER_LIB})
|
||||
# rpath на каталог pip libnvrtc — чтобы бинарник/модуль грузились без LD_LIBRARY_PATH
|
||||
set_target_properties(gpr_core_cpp PROPERTIES POSITION_INDEPENDENT_CODE ON
|
||||
BUILD_RPATH "${GPR_CUDA_LIB}" INSTALL_RPATH "${GPR_CUDA_LIB}")
|
||||
|
||||
if(GPR_BUILD_PYTHON)
|
||||
execute_process(COMMAND ${PYTHON_EXECUTABLE} -m pybind11 --cmakedir
|
||||
OUTPUT_VARIABLE PB11_DIR OUTPUT_STRIP_TRAILING_WHITESPACE RESULT_VARIABLE PB11_RC)
|
||||
if(PB11_RC EQUAL 0)
|
||||
find_package(pybind11 CONFIG REQUIRED HINTS ${PB11_DIR})
|
||||
else()
|
||||
find_package(pybind11 CONFIG REQUIRED)
|
||||
endif()
|
||||
pybind11_add_module(_gpr_cpp bindings/pymodule.cpp)
|
||||
target_link_libraries(_gpr_cpp PRIVATE gpr_core_cpp)
|
||||
set_target_properties(_gpr_cpp PROPERTIES BUILD_RPATH "${GPR_CUDA_LIB}" INSTALL_RPATH "${GPR_CUDA_LIB}")
|
||||
endif()
|
||||
|
||||
if(GPR_BUILD_TESTS)
|
||||
add_executable(gpr_test_parity tests/cpp/test_parity.cpp)
|
||||
target_link_libraries(gpr_test_parity PRIVATE gpr_core_cpp)
|
||||
endif()
|
||||
|
||||
if(GPR_BUILD_EXAMPLES)
|
||||
add_executable(standalone_bp examples/standalone_bp.cpp)
|
||||
target_link_libraries(standalone_bp PRIVATE gpr_core_cpp)
|
||||
set_target_properties(standalone_bp PROPERTIES BUILD_RPATH "${GPR_CUDA_LIB}" INSTALL_RPATH "${GPR_CUDA_LIB}")
|
||||
endif()
|
||||
@@ -0,0 +1,85 @@
|
||||
# gpr_cpp — C++/CUDA счёт-ядро MIMO GPR 3D SAR
|
||||
|
||||
> Инфраструктура (сборка, NVRTC, PocketFFT, npy) вендорена из репозитория
|
||||
> VNA_AlGo (`gpr_cpp/`, коммит `544dc59`) без python-биндингов
|
||||
> (`GPR_BUILD_PYTHON=OFF`). АЛГОРИТМ с сентября 2026 переписан: это порт
|
||||
> эталонного ноутбука `Multi_antenna_3D_clean_v1.ipynb` (ячейки 0 и 4), а не
|
||||
> старого `gpr_core`/Horns_3D. Паритет с ноутбуком проверяется инструментом
|
||||
> `sar3d_parity` + `python_app/scripts/sar3d_volume_compare.py` по
|
||||
> референсным `*_volume.npz`. Собирается из корневого Makefile radar_system
|
||||
> (цель `$(GPR_CPP_LIB)` вызывает `build_cmake.sh`).
|
||||
|
||||
## Что внутри
|
||||
|
||||
**В C++ (`libgpr_core_cpp`):** сегментированный тайминг кадра по per-trace
|
||||
меткам захвата (`compute_pair_timestamps`), препроцессинг A-сканов (bg-subtract,
|
||||
полная фазовая коррекция движения intra+pair до IFFT, k0-вставка + Ханн + IFFT
|
||||
через PocketFFT, нормировка пар), полнообластная ground-сетка
|
||||
(`build_full_region_grid`), 3D SAR апертурный объём (одно CUDA-ядро на всю
|
||||
апертуру; геометрия в double, A-сканы float32) и финализация
|
||||
(`finish_volume`: |acc/cnt| → маска глубины → нормировка без Y-краёв → гаусс
|
||||
по (Z,X) → нормировка; возвращает объём [0,1] + level).
|
||||
|
||||
## Ключевое решение: CUDA без nvcc (NVRTC + Driver API)
|
||||
|
||||
На целевых машинах (x86+T600, Jetson) нет полного CUDA toolkit — только runtime +
|
||||
**NVRTC** (как у cupy). Поэтому ядро **не компилируется nvcc'ом**: его исходник
|
||||
встроен строкой ([src/kernel_src.hpp](src/kernel_src.hpp)), компилируется в рантайме
|
||||
через NVRTC в **cubin под точную арх устройства** (запрос compute capability →
|
||||
`sm_XY`, без JIT PTX), грузится и запускается через **CUDA Driver API**. Хост-часть
|
||||
собирается обычным g++. Плюс: без toolkit и без привязки к SM — один код для
|
||||
x86 (sm_75) и Jetson (sm_87).
|
||||
|
||||
## Сборка
|
||||
|
||||
```bash
|
||||
bash build_cmake.sh # авто-детект pip CUDA-путей; собирает всё
|
||||
# артефакты в build/: libgpr_core_cpp.a, _gpr_cpp*.so, standalone_bp
|
||||
```
|
||||
Требуется: g++ ≥13, cmake ≥3.24, pybind11 (`pip install pybind11`), pip-пакеты
|
||||
`nvidia-*` (nvrtc+cudart, идут с cupy), системный `libcuda`. rpath на каталог
|
||||
libnvrtc прописан — бинарники/модуль грузятся без `LD_LIBRARY_PATH`.
|
||||
|
||||
## Публичный C++ API ([include/gpr/](include/gpr/))
|
||||
|
||||
```cpp
|
||||
#include "gpr/config.hpp" // GeometryConfig/BPConfig/MotionConfig/SARConfig (дефолты = ноутбук)
|
||||
#include "gpr/ascan.hpp" // compute_pair_timestamps, preprocess_snapshot(...) -> AScanSet
|
||||
#include "gpr/sar3d.hpp" // SarEngine::aperture_volume -> VolumeRaw; finish_volume -> FinishedVolume
|
||||
#include "gpr/geometry.hpp" // frame_offsets, frame_weights, build_full_region_grid
|
||||
|
||||
gpr::SarEngine engine(geom, bp, mot, sar); // компилит ядро под устройство
|
||||
auto raw = engine.aperture_volume(frames, grid); // один запуск ядра на апертуру
|
||||
auto fin = gpr::finish_volume(raw, grid, mot.pitch_deg, bp, sar); // объём [0,1] + level
|
||||
```
|
||||
Пример без Python: [examples/standalone_bp.cpp](examples/standalone_bp.cpp) — грузит
|
||||
S21 из `.npy`, строит объём, печатает максимум.
|
||||
```bash
|
||||
./build/standalone_bp <preprocessed_dir> 0038_id39_ns11361438137486
|
||||
```
|
||||
|
||||
## Python-модуль
|
||||
|
||||
```python
|
||||
import _gpr_cpp # из gpr_cpp/build
|
||||
eng = _gpr_cpp.SarEngine(_gpr_cpp.GeometryConfig(), _gpr_cpp.BPConfig(),
|
||||
_gpr_cpp.MotionConfig(), _gpr_cpp.SARConfig())
|
||||
a = _gpr_cpp.preprocess_snapshot(s21, freq, background, mot, geom, bp, True) # -> AScanSet
|
||||
acc, cnt = eng.aperture_volume([(a, dx, dy, dz, w), ...], gx, gy, gz) # numpy
|
||||
```
|
||||
|
||||
## Паритет с ноутбуком (проверено на x86 T600, датасет 20260922_mine_fast_true)
|
||||
|
||||
Кадры 0034/0035/0036 против референсных `*_volume.npz` ноутбука
|
||||
(`make sar3d_parity` + `python_app/scripts/sar3d_volume_compare.py`):
|
||||
max|Δ| ≤ 1.6e-6 вне ножевой границы маски глубины, argmax и level совпадают
|
||||
(level rel ≤ 7e-7). Python-эталон той же цепочки:
|
||||
`python_app/scripts/sar3d_reference_check.py`.
|
||||
|
||||
## Статус
|
||||
|
||||
- [x] x86 (T600, sm_75, CUDA 13): собрано, паритет, standalone работает.
|
||||
- [ ] Jetson (Orin NX, sm_87, CUDA 12.6): порт тривиален (NVRTC сам берёт sm_87);
|
||||
собрать `build_cmake.sh` на Jetson с его venv.
|
||||
- Опц.: ветка `impl='cpp'` в `gpr_core/sar3d.py` (адаптер `gpr_cpp_backend.py`),
|
||||
кэш device-A-сканов (сейчас каждый вызов грузит стек на GPU).
|
||||
@@ -0,0 +1,87 @@
|
||||
#!/usr/bin/env bash
|
||||
# CMake-сборка libgpr_core_cpp + standalone (без nvcc; NVRTC/driver).
|
||||
#
|
||||
# Источник CUDA (nvrtc.h / libnvrtc), по убыванию приоритета:
|
||||
# 1. env CUDA_INC / CUDA_LIB — заданы явно;
|
||||
# 2. pip-пакеты nvidia-* в venv ($VENV, по умолчанию ../../.venv — venv
|
||||
# radar_system) — дев-машина;
|
||||
# 3. системный toolkit /usr/local/cuda — Jetson/JetPack, apt cuda-toolkit.
|
||||
#
|
||||
# GPR_BUILD_PYTHON=ON вернул бы pybind-модуль, но bindings/ не вендорен в
|
||||
# radar_system (см. README) — по умолчанию OFF.
|
||||
set -eu
|
||||
HERE="$(cd "$(dirname "$0")" && pwd)"
|
||||
VENV="${VENV:-$HERE/../../.venv}"
|
||||
PY="$VENV/bin/python"
|
||||
GPR_BUILD_PYTHON="${GPR_BUILD_PYTHON:-OFF}"
|
||||
|
||||
CUDA_INC="${CUDA_INC:-}"
|
||||
CUDA_LIB="${CUDA_LIB:-}"
|
||||
|
||||
if [ -z "$CUDA_INC" ] || [ -z "$CUDA_LIB" ]; then
|
||||
if [ -x "$PY" ]; then
|
||||
# Печатается только когда найдены nvrtc.h, cuda.h И библиотека — частичная
|
||||
# детекция не может подсунуть мусорный путь. nvrtc.h и cuda.h могут лежать
|
||||
# в РАЗНЫХ каталогах (раздельные pip-пакеты cuda_nvrtc/cuda_runtime на
|
||||
# Jetson) — тогда CUDA_INC становится ';'-списком для CMake.
|
||||
PIP_CUDA=$("$PY" - <<'P' 2>/dev/null || true
|
||||
import glob, os, site
|
||||
sp = site.getsitepackages()[0]
|
||||
nvrtc_hdr = glob.glob(os.path.join(sp, 'nvidia', '**', 'include', 'nvrtc.h'), recursive=True)
|
||||
cuda_hdr = glob.glob(os.path.join(sp, 'nvidia', '**', 'include', 'cuda.h'), recursive=True)
|
||||
lib = sorted(glob.glob(os.path.join(sp, 'nvidia', '**', 'lib', 'libnvrtc.so*'), recursive=True))
|
||||
if nvrtc_hdr and cuda_hdr and lib:
|
||||
inc_dirs = []
|
||||
for hdr in (nvrtc_hdr[0], cuda_hdr[0]):
|
||||
d = os.path.dirname(hdr)
|
||||
if d not in inc_dirs:
|
||||
inc_dirs.append(d)
|
||||
print(';'.join(inc_dirs))
|
||||
print(os.path.dirname(lib[0]))
|
||||
P
|
||||
)
|
||||
if [ -n "$PIP_CUDA" ]; then
|
||||
CUDA_INC=$(printf '%s\n' "$PIP_CUDA" | sed -n 1p)
|
||||
CUDA_LIB=$(printf '%s\n' "$PIP_CUDA" | sed -n 2p)
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ -z "$CUDA_INC" ] || [ -z "$CUDA_LIB" ]; then
|
||||
if [ -e /usr/local/cuda/include/nvrtc.h ]; then
|
||||
CUDA_INC=/usr/local/cuda/include
|
||||
CUDA_LIB=/usr/local/cuda/lib64
|
||||
echo "[build_cmake] pip nvidia-* не найдены; использую системный CUDA: $CUDA_INC"
|
||||
else
|
||||
echo "[build_cmake] CUDA не найден: нет ни pip nvidia-* в $VENV, ни /usr/local/cuda." >&2
|
||||
echo "[build_cmake] Задайте CUDA_INC=<dir с nvrtc.h,cuda.h> и CUDA_LIB=<dir с libnvrtc.so*>." >&2
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
PYTHON_ARG=()
|
||||
if [ "$GPR_BUILD_PYTHON" = "ON" ] && [ -x "$PY" ]; then
|
||||
PYTHON_ARG=(-DPYTHON_EXECUTABLE="$PY")
|
||||
fi
|
||||
|
||||
# cmake: сначала из venv (pip install cmake — свежий без sudo), потом системный.
|
||||
CMAKE="$VENV/bin/cmake"
|
||||
if [ ! -x "$CMAKE" ]; then
|
||||
CMAKE=$(command -v cmake || true)
|
||||
fi
|
||||
if [ -z "$CMAKE" ]; then
|
||||
echo "[build_cmake] cmake не найден. Поставьте его в venv: \"$VENV/bin/pip\" install cmake" >&2
|
||||
echo "[build_cmake] (или системно: sudo apt install cmake)" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
"$CMAKE" -S "$HERE" -B "$HERE/build" \
|
||||
${PYTHON_ARG[@]+"${PYTHON_ARG[@]}"} \
|
||||
-DGPR_BUILD_PYTHON="$GPR_BUILD_PYTHON" \
|
||||
-DGPR_CUDA_INC="$CUDA_INC" \
|
||||
-DGPR_CUDA_LIB="$CUDA_LIB" \
|
||||
-DCMAKE_BUILD_TYPE=Release "$@" 2>&1 | tail -8
|
||||
"$CMAKE" --build "$HERE/build" -j4 2>&1 | tail -15
|
||||
echo "--- артефакты ---"
|
||||
ls -la "$HERE/build/libgpr_core_cpp.a" "$HERE/build/standalone_bp" "$HERE"/build/_gpr_cpp*.so 2>/dev/null | awk '{print $5,$9}'
|
||||
echo "CUDA_LIB=$CUDA_LIB (для LD_LIBRARY_PATH при запуске)"
|
||||
@@ -0,0 +1,159 @@
|
||||
// Автономный пример: C++ без Python. Грузит S21 снапшота из .npy,
|
||||
// строит A-сканы и 3D SAR-объём апертуры, печатает максимум |acc/cnt|.
|
||||
//
|
||||
// Использование:
|
||||
// standalone_bp <preprocessed_dir> <center_snapshot_name>
|
||||
// где preprocessed_dir содержит папки снапшотов с i{rx}_o{tx}_s21.npy и _freq.npy.
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <dirent.h>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/ascan.hpp"
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/geometry.hpp"
|
||||
#include "gpr/sar3d.hpp"
|
||||
|
||||
using namespace gpr;
|
||||
|
||||
// --- минимальный загрузчик numpy .npy (v1.0, 1-D complex128 или float64) ---
|
||||
struct NpyArray { std::vector<double> f64; std::vector<c128> c128v; bool is_complex = false; };
|
||||
|
||||
static NpyArray load_npy(const std::string& path) {
|
||||
std::ifstream in(path, std::ios::binary);
|
||||
if (!in) throw std::runtime_error("не открыть " + path);
|
||||
char magic[6]; in.read(magic, 6);
|
||||
uint8_t ver[2]; in.read((char*)ver, 2);
|
||||
uint16_t hlen; in.read((char*)&hlen, 2);
|
||||
std::string hdr(hlen, '\0'); in.read(&hdr[0], hlen);
|
||||
auto sp = hdr.find("'shape':");
|
||||
auto lp = hdr.find('(', sp), rp = hdr.find(')', sp);
|
||||
long n = std::stol(hdr.substr(lp + 1, rp - lp - 1));
|
||||
NpyArray a;
|
||||
auto has = [&](const char* s) { return hdr.find(s) != std::string::npos; };
|
||||
if (has("c8")) { // complex64 → c128
|
||||
a.is_complex = true; a.c128v.resize(n);
|
||||
std::vector<std::complex<float>> tmp(n);
|
||||
in.read((char*)tmp.data(), n * sizeof(std::complex<float>));
|
||||
for (long i = 0; i < n; ++i) a.c128v[i] = c128(tmp[i].real(), tmp[i].imag());
|
||||
} else if (has("c16")) { // complex128
|
||||
a.is_complex = true; a.c128v.resize(n);
|
||||
in.read((char*)a.c128v.data(), n * sizeof(c128));
|
||||
} else if (has("f4")) { // float32 → double
|
||||
a.f64.resize(n);
|
||||
std::vector<float> tmp(n); in.read((char*)tmp.data(), n * sizeof(float));
|
||||
for (long i = 0; i < n; ++i) a.f64[i] = tmp[i];
|
||||
} else { // float64
|
||||
a.f64.resize(n); in.read((char*)a.f64.data(), n * sizeof(double));
|
||||
}
|
||||
return a;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 3) { printf("usage: %s <preprocessed_dir> <center_snapshot>\n", argv[0]); return 1; }
|
||||
std::string root = argv[1], center = argv[2];
|
||||
|
||||
GeometryConfig geom; geom.finalize();
|
||||
BPConfig bp; MotionConfig mot; SARConfig sar;
|
||||
auto pairs = geom.pair_keys();
|
||||
auto tx = geom.output_idx, rx = geom.input_idx;
|
||||
std::sort(tx.begin(), tx.end()); std::sort(rx.begin(), rx.end());
|
||||
|
||||
// список снапшотов, выбор апертуры вокруг center
|
||||
std::vector<std::string> snaps;
|
||||
if (DIR* d = opendir(root.c_str())) {
|
||||
for (dirent* e; (e = readdir(d));) {
|
||||
std::string n = e->d_name;
|
||||
if (!n.empty() && isdigit(n[0])) snaps.push_back(n);
|
||||
}
|
||||
closedir(d);
|
||||
}
|
||||
std::sort(snaps.begin(), snaps.end());
|
||||
int ci = -1; for (int i = 0; i < (int)snaps.size(); ++i) if (snaps[i] == center) ci = i;
|
||||
if (ci < 0) { printf("центр %s не найден\n", center.c_str()); return 1; }
|
||||
int lo = std::max(0, ci - sar.frame_radius), hi = std::min((int)snaps.size(), ci + sar.frame_radius + 1);
|
||||
std::vector<std::string> aperture(snaps.begin() + lo, snaps.begin() + hi);
|
||||
printf("апертура: %d кадров вокруг %s\n", (int)aperture.size(), center.c_str());
|
||||
|
||||
auto load_snapshot = [&](const std::string& snap,
|
||||
std::map<PairKey, std::vector<c128>>& s21,
|
||||
std::map<PairKey, std::vector<double>>& freq) {
|
||||
for (int p = 0; p < (int)pairs.size(); ++p) {
|
||||
int i_tx = pairs[p].first, i_rx = pairs[p].second;
|
||||
int phys_tx = tx[i_tx], phys_rx = rx[i_rx];
|
||||
char fn[512];
|
||||
snprintf(fn, sizeof(fn), "%s/%s/i%d_o%d_s21.npy", root.c_str(), snap.c_str(), phys_rx, phys_tx);
|
||||
s21[{i_tx, i_rx}] = load_npy(fn).c128v;
|
||||
snprintf(fn, sizeof(fn), "%s/%s/i%d_o%d_freq.npy", root.c_str(), snap.c_str(), phys_rx, phys_tx);
|
||||
freq[{i_tx, i_rx}] = load_npy(fn).f64;
|
||||
}
|
||||
};
|
||||
|
||||
// Демо без чтения meta.json: равномерные сегменты по интервалу между
|
||||
// соседними снапшотами (реальные per-trace окна читает sar3d_parity).
|
||||
double acq_s = 0.0;
|
||||
{
|
||||
std::vector<double> stamps;
|
||||
for (auto& s : snaps) {
|
||||
auto pos = s.rfind("_ns");
|
||||
if (pos != std::string::npos) stamps.push_back(std::stod(s.substr(pos + 3)) * 1e-9);
|
||||
}
|
||||
std::sort(stamps.begin(), stamps.end());
|
||||
if (stamps.size() >= 2) acq_s = stamps[1] - stamps[0];
|
||||
}
|
||||
auto windows = uniform_tx_windows(geom.output_idx, acq_s, mot.tx_per_segment);
|
||||
auto ts = compute_pair_timestamps(windows, acq_s, geom.output_idx, geom, mot);
|
||||
SarEngine engine(geom, bp, mot, sar);
|
||||
|
||||
// держим AScanSet живыми
|
||||
std::vector<std::shared_ptr<AScanSet>> keep;
|
||||
std::vector<ApertureFrame> frames;
|
||||
std::vector<double> frame_times;
|
||||
for (auto& snap : aperture) {
|
||||
// время из суффикса _nsNNNN
|
||||
auto pos = snap.rfind("_ns");
|
||||
double t = (pos != std::string::npos) ? std::stod(snap.substr(pos + 3)) * 1e-9 : 0.0;
|
||||
frame_times.push_back(t);
|
||||
}
|
||||
double t_ref = frame_times[ci - lo];
|
||||
auto offs = frame_offsets(frame_times, t_ref, mot);
|
||||
auto wts = frame_weights(offs, sar.edge_frame_weight);
|
||||
|
||||
for (int f = 0; f < (int)aperture.size(); ++f) {
|
||||
std::map<PairKey, std::vector<c128>> s21;
|
||||
std::map<PairKey, std::vector<double>> freq;
|
||||
load_snapshot(aperture[f], s21, freq);
|
||||
auto a = std::make_shared<AScanSet>(
|
||||
preprocess_snapshot(s21, freq, nullptr, mot, geom, bp, ts, true));
|
||||
keep.push_back(a);
|
||||
frames.push_back(ApertureFrame{a.get(), offs[f], wts[f]});
|
||||
}
|
||||
|
||||
// ноутбучные границы сцены (размах антенн ±2 м, полоса глубин) вместо
|
||||
// фиксированного окна по умолчанию
|
||||
notebook_grid_bounds(geom, bp, sar, mot.pitch_deg, 2.0,
|
||||
sar.x_min_m, sar.x_max_m, sar.z_min_m, sar.z_max_m);
|
||||
Grid grid = build_full_region_grid(sar);
|
||||
printf("сетка %dx%dx%d, счёт объёма...\n", grid.NX(), grid.NY(), grid.NZ());
|
||||
|
||||
VolumeRaw vol = engine.aperture_volume(frames, grid);
|
||||
FinishedVolume fin = finish_volume(vol, grid, mot.pitch_deg, bp, sar);
|
||||
|
||||
double vmax = 0; size_t amax = 0;
|
||||
for (size_t i = 0; i < fin.volume.size(); ++i)
|
||||
if (fin.volume[i] > vmax) { vmax = fin.volume[i]; amax = i; }
|
||||
int NX = vol.NX, NZ = vol.NZ;
|
||||
int ix = amax % NX, iz = (amax / NX) % NZ, iy = amax / ((size_t)NX * NZ);
|
||||
printf("МАКСИМУМ объёма: %.4f (level %.4e) в вокселе (ix=%d,iy=%d,iz=%d)\n",
|
||||
vmax, fin.level, ix, iy, iz);
|
||||
printf(" X=%.1f см, Y=%.1f см, Z=%.1f см\n",
|
||||
grid.x[ix] * 100, grid.y[iy] * 100, grid.z[iz] * 100);
|
||||
printf("OK: автономный C++ путь отработал без Python.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
// Препроцессинг S21 → комплексные A-сканы (host, float64).
|
||||
// Порт ячейки 0 Multi_antenna_3D_clean_v1.ipynb (segmented_interleaved тайминг,
|
||||
// frequency-domain коррекция движения, k0-IFFT, нормировка пар).
|
||||
#pragma once
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/types.hpp"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
using PairKey = std::pair<int, int>;
|
||||
|
||||
// Тайминг пары в модели segmented_interleaved: пара живёт в сегменте своего Tx
|
||||
// (окно [segment_start, segment_start+segment_time] от начала акквизиции),
|
||||
// внутри сегмента передатчики чередуются по частоте (slot из n_slots).
|
||||
struct PairTiming {
|
||||
int tx_interleave_slot = 0;
|
||||
int tx_interleave_slots = 1;
|
||||
double segment_start_s = 0.0;
|
||||
double segment_time_s = 0.0;
|
||||
double t_center_s = 0.0; // центр окна сегмента
|
||||
// Грубый межсегментный сдвиг: dtau центра сегмента относительно центра
|
||||
// акквизиции кадра (2*dz/v, dz = dir*speed*dt*cos(look)).
|
||||
double dtau_pair_s = 0.0;
|
||||
};
|
||||
using PairTimestamps = std::map<PairKey, PairTiming>;
|
||||
|
||||
// Окна захвата по Tx (output_pos → [start,end] сек от начала акквизиции).
|
||||
using TxWindows = std::map<int, std::pair<double, double>>;
|
||||
|
||||
// Fallback без per-trace меток: сегменты по tx_per_segment штук делят кадр
|
||||
// [0, acq_s] поровну (медианная модель ноутбука compute_pair_timestamps).
|
||||
TxWindows uniform_tx_windows(const std::vector<int>& tx_order, double acq_s, int tx_per_segment);
|
||||
|
||||
// Порт compute_pair_timestamps_for_snapshot: Tx с одинаковым окном — один
|
||||
// сегмент (сегменты по возрастанию окна), слот — порядок в tx_order (полный
|
||||
// аппаратный порядок из run.combos; выбранные каналы его не меняют).
|
||||
PairTimestamps compute_pair_timestamps(const TxWindows& tx_windows_s, double acq_s,
|
||||
const std::vector<int>& tx_order,
|
||||
const GeometryConfig&, const MotionConfig&);
|
||||
|
||||
// A-сканы одного снапшота, общая ось времени у всех пар (assert).
|
||||
struct AScanSet {
|
||||
int n_pairs = 0;
|
||||
int n_fft = 0;
|
||||
double t0 = 0.0, dt = 0.0; // общая ось: t_k = t0 + k*dt
|
||||
std::vector<PairKey> pairs; // порядок как в geom.pair_keys()
|
||||
std::vector<c128> H; // [n_pairs * n_fft] float64
|
||||
const c128* pair_ptr(int p) const { return H.data() + (size_t)p * n_fft; }
|
||||
};
|
||||
|
||||
// Порт sar_pair_ascans: bg-subtract → phase correction → k0-IFFT → норма.
|
||||
// s21/freq/background — по ключам (i_tx,i_rx); background может быть nullptr.
|
||||
AScanSet preprocess_snapshot(
|
||||
const std::map<PairKey, std::vector<c128>>& s21,
|
||||
const std::map<PairKey, std::vector<double>>& freq,
|
||||
const std::map<PairKey, std::vector<c128>>* background,
|
||||
const MotionConfig&, const GeometryConfig&, const BPConfig&,
|
||||
const PairTimestamps&, bool normalize = true);
|
||||
|
||||
// Отдельные шаги (для тестов/переиспользования):
|
||||
// Полная фазовая модель движения до IFFT: dtau_intra(f) + dtau_pair одной
|
||||
// фазой exp(sign*j*2*pi*f*dtau_total) (порт apply_intra_sweep_phase_correction).
|
||||
std::vector<c128> apply_intra_sweep_phase_correction(
|
||||
const std::vector<c128>& s21, const std::vector<double>& freq_full,
|
||||
const PairTiming&, const MotionConfig&, double wave_speed,
|
||||
double f_start, double f_stop);
|
||||
|
||||
// Возвращает t (ось), h (комплексный A-скан). n_fft через out-параметр.
|
||||
void compute_ascan_bp(const std::vector<c128>& s21, const std::vector<double>& freq,
|
||||
double f_start, double f_stop, bool window, int oversample,
|
||||
std::vector<double>& t_out, std::vector<c128>& h_out);
|
||||
|
||||
double pair_norm_scale(const std::vector<c128>& h, const std::vector<double>& t,
|
||||
double wave_speed, const BPConfig&);
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,114 @@
|
||||
// Конфигурация счёт-ядра. Дефолты 1:1 повторяют Multi_antenna_3D_clean_v1.ipynb
|
||||
// (ячейки 0 и 4) — эталонный ноутбук 3D SAR.
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
struct GeometryConfig {
|
||||
std::vector<int> input_idx{0, 1, 2, 3};
|
||||
std::vector<int> output_idx{0, 1, 2, 3};
|
||||
// (x,y,z) метры; порядок соответствует input_idx/output_idx
|
||||
std::vector<std::array<double, 3>> tx_positions{
|
||||
{-0.88, 0.0, 0.0}, {0.89, 0.0, 0.0}, {-0.33, 0.445, 0.0}, {0.33, 0.445, 0.0}};
|
||||
std::vector<std::array<double, 3>> rx_positions{
|
||||
{0.23, 0.0, 0.0}, {0.54, 0.15, 0.0}, {-0.55, 0.3, 0.0}, {-0.23, 0.0, 0.0}};
|
||||
double eps_r = 1.0;
|
||||
|
||||
// --- производные (заполняются finalize) ---
|
||||
std::vector<double> x_tx, y_tx, z_tx; // отсортированы по phys-индексу
|
||||
std::vector<double> x_rx, y_rx, z_rx;
|
||||
int n_tx = 0, n_rx = 0;
|
||||
double v = 0.0;
|
||||
|
||||
void finalize() {
|
||||
auto sort_xyz = [](std::vector<int> idx, const std::vector<std::array<double, 3>>& pos,
|
||||
std::vector<double>& X, std::vector<double>& Y, std::vector<double>& Z) {
|
||||
std::vector<int> order(idx.size());
|
||||
for (size_t i = 0; i < idx.size(); ++i) order[i] = (int)i;
|
||||
std::sort(order.begin(), order.end(), [&](int a, int b) { return idx[a] < idx[b]; });
|
||||
X.clear(); Y.clear(); Z.clear();
|
||||
for (int o : order) { X.push_back(pos[o][0]); Y.push_back(pos[o][1]); Z.push_back(pos[o][2]); }
|
||||
};
|
||||
sort_xyz(output_idx, tx_positions, x_tx, y_tx, z_tx);
|
||||
sort_xyz(input_idx, rx_positions, x_rx, y_rx, z_rx);
|
||||
n_tx = (int)output_idx.size();
|
||||
n_rx = (int)input_idx.size();
|
||||
v = 3e8 / std::sqrt(eps_r);
|
||||
}
|
||||
|
||||
std::vector<std::pair<int, int>> pair_keys() const {
|
||||
std::vector<std::pair<int, int>> p;
|
||||
for (int i = 0; i < n_tx; ++i)
|
||||
for (int j = 0; j < n_rx; ++j) p.emplace_back(i, j);
|
||||
return p;
|
||||
}
|
||||
};
|
||||
|
||||
struct BPConfig {
|
||||
double f_start = 2.0e9;
|
||||
double f_stop = 6.0e9;
|
||||
double min_depth = 2.5;
|
||||
double max_depth = 8.0;
|
||||
int oversample = 8;
|
||||
bool window = true; // окно Ханна на полосе
|
||||
bool pair_normalize = true;
|
||||
double pair_norm_percentile = 50.0;
|
||||
double pair_norm_depth_min = 2.5;
|
||||
double pair_norm_depth_max = 8.0;
|
||||
double pair_norm_eps = 1e-15;
|
||||
// Компенсация геометрического расхождения 1/(Rtx*Rrx), нормированная на
|
||||
// точку под виртуальным центром пары на глубине comp_ref_depth НА ПЛОСКОСТИ
|
||||
// ЗЕМЛИ (высота (-ground_depth + d*sin p)/cos p).
|
||||
double comp_range_power = 0.3;
|
||||
double comp_weight_max = 5.0;
|
||||
double comp_ref_depth = 5.0;
|
||||
};
|
||||
|
||||
struct MotionConfig {
|
||||
double speed_m_s = 1.45;
|
||||
double look_angle_deg = 17.5;
|
||||
// Наклон рамы для ground-координат и смещений кадров; в ноутбуке
|
||||
// BP_GROUND_PITCH_DEG=None означает «равен look_angle».
|
||||
double pitch_deg = 17.5;
|
||||
double direction_sign = +1.0;
|
||||
// Внутрисвиповая (по частоте) фазовая коррекция движения.
|
||||
bool apply_freq_phase_correction = true;
|
||||
// Грубая межсегментная коррекция (dtau сегмента относительно центра кадра),
|
||||
// вносится той же фазовой моделью до IFFT (режим 'frequency_domain').
|
||||
bool apply_pair_motion_correction = true;
|
||||
double motion_phase_sign = -1.0;
|
||||
// Fallback-разбиение кадра на сегменты, когда per-trace меток захвата нет.
|
||||
int tx_per_segment = 2;
|
||||
};
|
||||
|
||||
struct SARConfig {
|
||||
int frame_radius = 0; // 0 = один снимок (физическая вертикальная база)
|
||||
double edge_frame_weight = 0.8; // вес крайнего кадра апертуры
|
||||
bool coord_mode_ground = true; // 'ground' vs 'radar_local'
|
||||
bool use_compensation = true;
|
||||
// Высота НИЖНЕГО ряда антенн над землёй; Y=0 сцены — поверхность земли.
|
||||
double ground_depth_m = 1.25;
|
||||
// Явные границы сцены [м]: X — поперёк, Y — высота над землёй, Z — вдаль.
|
||||
// Воксели с радарной глубиной вне [min_depth, max_depth] всё равно
|
||||
// обнуляются маской, так что Z-окно шире полосы глубин безопасно.
|
||||
double x_min_m = -2.0;
|
||||
double x_max_m = 2.0;
|
||||
double y_min_m = -0.5;
|
||||
double y_max_m = 2.0;
|
||||
double z_min_m = 0.0;
|
||||
double z_max_m = 8.0;
|
||||
std::array<double, 3> voxel_m{0.025, 0.02, 0.02}; // шаг (X, Y, Z)
|
||||
// Финализация объёма: нормировка без верхних/нижних norm_edge_margin_m по Y
|
||||
// (там ложный гребень постоянной дальности), сглаживание по (Z,X) в каждом
|
||||
// Y-плане.
|
||||
double norm_edge_margin_m = 0.3;
|
||||
double smooth_sigma = 1.5;
|
||||
};
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,52 @@
|
||||
// CUDA Driver API RAII: контекст, поток, буфер устройства.
|
||||
#pragma once
|
||||
#include <cuda.h>
|
||||
|
||||
#include <cstddef>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
void cu_check(CUresult r, const char* what);
|
||||
|
||||
// Инициализация драйвера + primary context на device 0. Возвращает (major,minor) CC.
|
||||
struct CudaContext {
|
||||
CUdevice dev = 0;
|
||||
CUcontext ctx = nullptr;
|
||||
int cc_major = 0, cc_minor = 0;
|
||||
CudaContext();
|
||||
~CudaContext();
|
||||
std::string arch_flag() const; // "--gpu-architecture=sm_XY"
|
||||
};
|
||||
|
||||
template <class T>
|
||||
class DeviceBuffer {
|
||||
CUdeviceptr ptr_ = 0;
|
||||
size_t n_ = 0;
|
||||
|
||||
public:
|
||||
DeviceBuffer() = default;
|
||||
explicit DeviceBuffer(size_t n) { alloc(n); }
|
||||
~DeviceBuffer() { free(); }
|
||||
DeviceBuffer(const DeviceBuffer&) = delete;
|
||||
DeviceBuffer& operator=(const DeviceBuffer&) = delete;
|
||||
DeviceBuffer(DeviceBuffer&& o) noexcept : ptr_(o.ptr_), n_(o.n_) { o.ptr_ = 0; o.n_ = 0; }
|
||||
|
||||
void alloc(size_t n) {
|
||||
free();
|
||||
if (n) cu_check(cuMemAlloc(&ptr_, n * sizeof(T)), "cuMemAlloc");
|
||||
n_ = n;
|
||||
}
|
||||
void free() {
|
||||
if (ptr_) cuMemFree(ptr_);
|
||||
ptr_ = 0; n_ = 0;
|
||||
}
|
||||
void upload(const T* host, size_t n) { cu_check(cuMemcpyHtoD(ptr_, host, n * sizeof(T)), "HtoD"); }
|
||||
void download(T* host, size_t n) const { cu_check(cuMemcpyDtoH(host, ptr_, n * sizeof(T)), "DtoH"); }
|
||||
CUdeviceptr ptr() const { return ptr_; }
|
||||
CUdeviceptr* addr() { return &ptr_; }
|
||||
size_t size() const { return n_; }
|
||||
};
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,41 @@
|
||||
// Геометрия: опорное расхождение пары, смещения/веса кадров, построение сетки.
|
||||
#pragma once
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/types.hpp"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
// 1/geo_ref пары (порт pair_geo_ref, скаляр float64).
|
||||
double pair_geo_ref(const GeometryConfig&, const BPConfig&, int i_tx, int i_rx, double yy);
|
||||
|
||||
// Смещения кадров относительно опорного (порт sar_frame_offsets).
|
||||
// frame_times[k] — время кадра (сек, от ns-таймстемпа); t_ref — время опорного.
|
||||
std::vector<FrameOffset> frame_offsets(const std::vector<double>& frame_times, double t_ref,
|
||||
const MotionConfig&);
|
||||
|
||||
// Косинусные веса апертуры (порт sar_frame_weights); dt берётся относительно центра.
|
||||
std::vector<double> frame_weights(const std::vector<FrameOffset>& offsets, double edge_weight);
|
||||
|
||||
// Высота плоскости земли (радарные оси) на радарной глубине depth:
|
||||
// y = (-ground_depth + depth*sin(pitch)) / cos(pitch) (порт bp_plane_y).
|
||||
double ground_plane_y(double depth, double ground_depth_m, double pitch_rad);
|
||||
|
||||
// Сетка сцены по явным границам SARConfig (x/y/z_min..max, шаг voxel_m);
|
||||
// оси — linspace c числом точек round(span/step)+1, как в ноутбуке.
|
||||
Grid build_full_region_grid(const SARConfig&);
|
||||
|
||||
// Ноутбучные границы (sar_build_full_region_grid): X — размах антенн ±margin,
|
||||
// Z — полоса, покрывающая радарные глубины [min_depth, max_depth] на всех
|
||||
// высотах Y. Используются parity-инструментом для сверки с эталонными npz.
|
||||
void notebook_grid_bounds(const GeometryConfig&, const BPConfig&, const SARConfig&,
|
||||
double pitch_deg, double x_margin_m,
|
||||
double& x_min, double& x_max, double& z_min, double& z_max);
|
||||
|
||||
// point_to_scene: оси радара → сцена (поворот на pitch + подъём на ground_depth,
|
||||
// если ground; Y=0 сцены — земля).
|
||||
std::array<double, 3> point_to_scene(double x, double y, double z, const MotionConfig&,
|
||||
const SARConfig&);
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,25 @@
|
||||
// Компиляция строки CUDA-ядра через NVRTC → cubin под арх устройства → CUfunction.
|
||||
#pragma once
|
||||
#include <cuda.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
class NvrtcKernel {
|
||||
CUmodule module_ = nullptr;
|
||||
CUfunction func_ = nullptr;
|
||||
|
||||
public:
|
||||
NvrtcKernel() = default;
|
||||
~NvrtcKernel();
|
||||
NvrtcKernel(const NvrtcKernel&) = delete;
|
||||
NvrtcKernel& operator=(const NvrtcKernel&) = delete;
|
||||
|
||||
// Компилирует src в cubin под arch_flag ("--gpu-architecture=sm_XY"),
|
||||
// загружает модуль и берёт функцию func_name.
|
||||
void compile(const char* src, const std::string& arch_flag, const char* func_name);
|
||||
CUfunction func() const { return func_; }
|
||||
};
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,55 @@
|
||||
// 3D SAR: сборка взвешенного объёма апертуры за один запуск ядра.
|
||||
#pragma once
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/ascan.hpp"
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/device.hpp"
|
||||
#include "gpr/geometry.hpp"
|
||||
#include "gpr/nvrtc_kernel.hpp"
|
||||
#include "gpr/types.hpp"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
// Один кадр апертуры: A-сканы + смещение + вес.
|
||||
struct ApertureFrame {
|
||||
const AScanSet* ascans;
|
||||
FrameOffset offset;
|
||||
double weight;
|
||||
};
|
||||
|
||||
// Сырой результат объёма (до finish_volume).
|
||||
struct VolumeRaw {
|
||||
std::vector<c64> acc; // [NY*NZ*NX]
|
||||
std::vector<float> cnt; // [NY*NZ*NX]
|
||||
int NX = 0, NY = 0, NZ = 0;
|
||||
};
|
||||
|
||||
// Финальный объём (порт sar_finish_volume): |acc/cnt| → маска глубины →
|
||||
// нормировка по Y-планам без краёв → гаусс по (Z,X) → нормировка → clip.
|
||||
struct FinishedVolume {
|
||||
std::vector<float> volume; // [NY*NZ*NX], нормирован на ~[0,1]
|
||||
double level = 0.0; // масштаб, на который поделён объём
|
||||
int NX = 0, NY = 0, NZ = 0;
|
||||
};
|
||||
|
||||
FinishedVolume finish_volume(const VolumeRaw&, const Grid&, double pitch_deg,
|
||||
const BPConfig&, const SARConfig&);
|
||||
|
||||
class SarEngine {
|
||||
public:
|
||||
SarEngine(const GeometryConfig&, const BPConfig&, const MotionConfig&, const SARConfig&);
|
||||
// Весь объём апертуры за один запуск ядра; возвращает сырые acc/cnt (host).
|
||||
VolumeRaw aperture_volume(const std::vector<ApertureFrame>& frames, const Grid&);
|
||||
|
||||
private:
|
||||
GeometryConfig geom_;
|
||||
BPConfig bp_;
|
||||
MotionConfig mot_;
|
||||
SARConfig sar_;
|
||||
std::unique_ptr<CudaContext> ctx_;
|
||||
std::unique_ptr<NvrtcKernel> kernel_;
|
||||
};
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,25 @@
|
||||
// Базовые типы счёт-ядра.
|
||||
#pragma once
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
using c64 = std::complex<float>; // complex64, layout == cuComplex (2×float)
|
||||
using c128 = std::complex<double>; // float64 для препроцессинга/IFFT
|
||||
|
||||
// Ground-scene сетка (порт dict из geometry.py: x,y,z оси в метрах).
|
||||
struct Grid {
|
||||
std::vector<double> x, y, z;
|
||||
int NX() const { return (int)x.size(); }
|
||||
int NY() const { return (int)y.size(); }
|
||||
int NZ() const { return (int)z.size(); }
|
||||
};
|
||||
|
||||
// Смещение антенн кадра апертуры (порт sar_frame_offsets).
|
||||
struct FrameOffset {
|
||||
double dx_m = 0.0, dy_m = 0.0, dz_m = 0.0, dt_s = 0.0, horizontal_m = 0.0;
|
||||
};
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,250 @@
|
||||
#include "gpr/ascan.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "pocketfft_hdronly.h"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
TxWindows uniform_tx_windows(const std::vector<int>& tx_order, double acq_s, int tx_per_segment) {
|
||||
TxWindows windows;
|
||||
if (tx_order.empty()) return windows;
|
||||
int per_seg = std::max(1, tx_per_segment);
|
||||
int n_seg = std::max(1, (int)((tx_order.size() + per_seg - 1) / per_seg));
|
||||
double t_seg = acq_s / n_seg;
|
||||
for (size_t k = 0; k < tx_order.size(); ++k) {
|
||||
int seg = (int)k / per_seg;
|
||||
windows[tx_order[k]] = {seg * t_seg, seg * t_seg + t_seg};
|
||||
}
|
||||
return windows;
|
||||
}
|
||||
|
||||
PairTimestamps compute_pair_timestamps(const TxWindows& tx_windows_s, double acq_s,
|
||||
const std::vector<int>& tx_order,
|
||||
const GeometryConfig& geom, const MotionConfig& mot) {
|
||||
// Аппаратный порядок: сперва tx_order (run.combos), затем прочие ключи окон.
|
||||
std::vector<int> hw_order;
|
||||
for (int tx : tx_order)
|
||||
if (tx_windows_s.count(tx) && std::find(hw_order.begin(), hw_order.end(), tx) == hw_order.end())
|
||||
hw_order.push_back(tx);
|
||||
for (auto& [tx, _] : tx_windows_s)
|
||||
if (std::find(hw_order.begin(), hw_order.end(), tx) == hw_order.end())
|
||||
hw_order.push_back(tx);
|
||||
|
||||
// Tx с одинаковым окном — один сегмент; сегменты по возрастанию окна,
|
||||
// слот — порядок появления в hw_order (порт группировки ноутбука).
|
||||
std::vector<std::pair<std::pair<double, double>, std::vector<int>>> groups;
|
||||
for (int tx : hw_order) {
|
||||
auto win = tx_windows_s.at(tx);
|
||||
auto found = std::find_if(groups.begin(), groups.end(),
|
||||
[&](const auto& g) { return g.first == win; });
|
||||
if (found == groups.end())
|
||||
groups.push_back({win, {tx}});
|
||||
else
|
||||
found->second.push_back(tx);
|
||||
}
|
||||
std::sort(groups.begin(), groups.end(),
|
||||
[](const auto& a, const auto& b) { return a.first < b.first; });
|
||||
|
||||
struct SegSlot { double t0, t1; int slot, n_slots; };
|
||||
std::map<int, SegSlot> seg_of;
|
||||
for (auto& [win, txs] : groups)
|
||||
for (size_t slot = 0; slot < txs.size(); ++slot)
|
||||
seg_of[txs[slot]] = {win.first, win.second, (int)slot, (int)txs.size()};
|
||||
|
||||
double cos_theta = std::cos(mot.look_angle_deg * M_PI / 180.0);
|
||||
double t_ref = 0.5 * acq_s;
|
||||
PairTimestamps ts;
|
||||
for (int i = 0; i < geom.n_tx; ++i) {
|
||||
auto it = seg_of.find(geom.output_idx[i]);
|
||||
if (it == seg_of.end())
|
||||
throw std::runtime_error("pair timestamps: нет окна для Tx");
|
||||
const SegSlot& s = it->second;
|
||||
PairTiming timing;
|
||||
timing.tx_interleave_slot = s.slot;
|
||||
timing.tx_interleave_slots = s.n_slots;
|
||||
timing.segment_start_s = s.t0;
|
||||
timing.segment_time_s = s.t1 - s.t0;
|
||||
timing.t_center_s = 0.5 * (s.t0 + s.t1);
|
||||
double dz = mot.direction_sign * mot.speed_m_s * (timing.t_center_s - t_ref) * cos_theta;
|
||||
timing.dtau_pair_s = 2.0 * dz / geom.v;
|
||||
for (int j = 0; j < geom.n_rx; ++j) ts[{i, j}] = timing;
|
||||
}
|
||||
return ts;
|
||||
}
|
||||
|
||||
static double median_of_diff(const std::vector<double>& f) {
|
||||
std::vector<double> d(f.size() - 1);
|
||||
for (size_t i = 0; i + 1 < f.size(); ++i) d[i] = f[i + 1] - f[i];
|
||||
std::sort(d.begin(), d.end());
|
||||
size_t n = d.size();
|
||||
return (n % 2) ? d[n / 2] : 0.5 * (d[n / 2 - 1] + d[n / 2]);
|
||||
}
|
||||
|
||||
std::vector<c128> apply_intra_sweep_phase_correction(
|
||||
const std::vector<c128>& s21, const std::vector<double>& freq_full,
|
||||
const PairTiming& info, const MotionConfig& cfg, double wave_speed,
|
||||
double f_start, double f_stop) {
|
||||
std::vector<c128> out = s21;
|
||||
int n_full = (int)freq_full.size();
|
||||
// индексы в полосе
|
||||
std::vector<int> idx;
|
||||
for (int i = 0; i < n_full; ++i)
|
||||
if (freq_full[i] >= f_start && freq_full[i] <= f_stop) idx.push_back(i);
|
||||
if (idx.empty()) throw std::runtime_error("phase corr: пустая полоса");
|
||||
if (n_full < 2) return out;
|
||||
|
||||
// Сегмент проходит все n_full частот за своё окно; на каждой частоте по
|
||||
// очереди работают его n_slots передатчиков, поэтому событие частоты i для
|
||||
// слота slot имеет номер (n_slots*i + slot) из (n_slots*n_full).
|
||||
int n_slots = std::max(1, info.tx_interleave_slots);
|
||||
int slot = info.tx_interleave_slot;
|
||||
double dt_event = info.segment_time_s / ((double)n_slots * n_full);
|
||||
double cos_theta = std::cos(cfg.look_angle_deg * M_PI / 180.0);
|
||||
double pair_dtau = cfg.apply_pair_motion_correction ? info.dtau_pair_s : 0.0;
|
||||
if (!cfg.apply_freq_phase_correction && pair_dtau == 0.0) return out;
|
||||
for (int i : idx) {
|
||||
double dtau_intra = 0.0;
|
||||
if (cfg.apply_freq_phase_correction) {
|
||||
double t_abs = info.segment_start_s + (double)(n_slots * i + slot) * dt_event;
|
||||
double dt_intra = t_abs - info.t_center_s;
|
||||
dtau_intra = 2.0 * cfg.direction_sign * cfg.speed_m_s * cos_theta * dt_intra / wave_speed;
|
||||
}
|
||||
double phi = 2.0 * M_PI * freq_full[i] * (dtau_intra + pair_dtau);
|
||||
double ang = cfg.motion_phase_sign * phi;
|
||||
out[i] *= c128(std::cos(ang), std::sin(ang));
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
static int next_pow2(int x) {
|
||||
int p = 1;
|
||||
while (p < x) p <<= 1;
|
||||
return p;
|
||||
}
|
||||
|
||||
void compute_ascan_bp(const std::vector<c128>& s21, const std::vector<double>& freq,
|
||||
double f_start, double f_stop, bool window, int oversample,
|
||||
std::vector<double>& t_out, std::vector<c128>& h_out) {
|
||||
std::vector<double> freq_cut;
|
||||
std::vector<c128> s_cut;
|
||||
for (size_t i = 0; i < freq.size(); ++i)
|
||||
if (freq[i] >= f_start && freq[i] <= f_stop) {
|
||||
freq_cut.push_back(freq[i]);
|
||||
s_cut.push_back(s21[i]);
|
||||
}
|
||||
if (freq_cut.size() < 2) throw std::runtime_error("ascan: <2 точек в полосе");
|
||||
|
||||
double df = median_of_diff(freq_cut);
|
||||
int n = (int)freq_cut.size();
|
||||
int k0 = (int)std::nearbyint(freq_cut[0] / df); // round-half-to-even как python round
|
||||
int min_len = 2 * (k0 + n - 1);
|
||||
int n_fft_base = next_pow2(min_len);
|
||||
int n_fft = n_fft_base * oversample;
|
||||
double dt = 1.0 / ((double)n_fft * df);
|
||||
|
||||
// окно Ханна (numpy np.hanning: 0.5 - 0.5*cos(2*pi*i/(N-1)))
|
||||
std::vector<c128> H(n_fft, c128(0.0, 0.0));
|
||||
for (int i = 0; i < n; ++i) {
|
||||
c128 v = s_cut[i];
|
||||
if (window) {
|
||||
double w = 0.5 - 0.5 * std::cos(2.0 * M_PI * i / (n - 1));
|
||||
v *= w;
|
||||
}
|
||||
H[k0 + i] = v;
|
||||
}
|
||||
|
||||
// IFFT через pocketfft (backward + 1/n_fft == numpy ifft)
|
||||
h_out.assign(n_fft, c128(0.0, 0.0));
|
||||
pocketfft::shape_t shape{(size_t)n_fft};
|
||||
pocketfft::stride_t stride{(ptrdiff_t)sizeof(c128)};
|
||||
pocketfft::c2c(shape, stride, stride, {0}, pocketfft::BACKWARD,
|
||||
H.data(), h_out.data(), (double)(1.0 / n_fft));
|
||||
|
||||
t_out.resize(n_fft);
|
||||
for (int i = 0; i < n_fft; ++i) t_out[i] = i * dt;
|
||||
}
|
||||
|
||||
double pair_norm_scale(const std::vector<c128>& h, const std::vector<double>& t,
|
||||
double wave_speed, const BPConfig& bp) {
|
||||
std::vector<double> vals;
|
||||
vals.reserve(h.size());
|
||||
for (size_t i = 0; i < h.size(); ++i) {
|
||||
double z = t[i] * wave_speed / 2.0;
|
||||
if (z >= bp.pair_norm_depth_min && z <= bp.pair_norm_depth_max)
|
||||
vals.push_back(std::abs(h[i]));
|
||||
}
|
||||
if (vals.empty())
|
||||
for (auto& c : h) vals.push_back(std::abs(c));
|
||||
std::sort(vals.begin(), vals.end());
|
||||
// numpy.percentile linear interpolation
|
||||
double q = bp.pair_norm_percentile / 100.0;
|
||||
double pos = q * (vals.size() - 1);
|
||||
int lo = (int)std::floor(pos);
|
||||
int hi = std::min(lo + 1, (int)vals.size() - 1);
|
||||
double scale = vals[lo] + (pos - lo) * (vals[hi] - vals[lo]);
|
||||
if (!std::isfinite(scale) || scale <= bp.pair_norm_eps) scale = 1.0;
|
||||
return scale;
|
||||
}
|
||||
|
||||
AScanSet preprocess_snapshot(
|
||||
const std::map<PairKey, std::vector<c128>>& s21,
|
||||
const std::map<PairKey, std::vector<double>>& freq,
|
||||
const std::map<PairKey, std::vector<c128>>* background,
|
||||
const MotionConfig& mot, const GeometryConfig& geom, const BPConfig& bp,
|
||||
const PairTimestamps& ts, bool normalize) {
|
||||
AScanSet out;
|
||||
out.pairs = geom.pair_keys();
|
||||
out.n_pairs = (int)out.pairs.size();
|
||||
|
||||
std::vector<std::vector<c128>> Hs;
|
||||
std::vector<std::vector<double>> Ts;
|
||||
for (auto& key : out.pairs) {
|
||||
auto its = s21.find(key);
|
||||
if (its == s21.end()) throw std::runtime_error("preprocess: нет пары в s21");
|
||||
std::vector<c128> s = its->second; // копия
|
||||
if (background) {
|
||||
auto ib = background->find(key);
|
||||
if (ib != background->end())
|
||||
for (size_t i = 0; i < s.size(); ++i) s[i] -= ib->second[i];
|
||||
}
|
||||
const std::vector<double>& fr = freq.at(key);
|
||||
auto itt = ts.find(key);
|
||||
if (itt != ts.end())
|
||||
s = apply_intra_sweep_phase_correction(s, fr, itt->second, mot, geom.v,
|
||||
bp.f_start, bp.f_stop);
|
||||
std::vector<double> t;
|
||||
std::vector<c128> h;
|
||||
compute_ascan_bp(s, fr, bp.f_start, bp.f_stop, bp.window, bp.oversample, t, h);
|
||||
Hs.push_back(std::move(h));
|
||||
Ts.push_back(std::move(t));
|
||||
}
|
||||
|
||||
// общая ось времени — проверка
|
||||
int n_fft = (int)Ts[0].size();
|
||||
double t0 = Ts[0][0];
|
||||
double dt = (Ts[0].back() - t0) / (n_fft - 1);
|
||||
for (auto& t : Ts)
|
||||
if ((int)t.size() != n_fft || std::abs(t[0] - t0) > 1e-18)
|
||||
throw std::runtime_error("A-сканы: разная ось времени у пар");
|
||||
|
||||
if (normalize) {
|
||||
for (int p = 0; p < out.n_pairs; ++p) {
|
||||
double scale = pair_norm_scale(Hs[p], Ts[p], geom.v, bp);
|
||||
for (auto& c : Hs[p]) c /= (scale + bp.pair_norm_eps);
|
||||
}
|
||||
}
|
||||
|
||||
out.n_fft = n_fft;
|
||||
out.t0 = t0;
|
||||
out.dt = dt;
|
||||
out.H.resize((size_t)out.n_pairs * n_fft);
|
||||
for (int p = 0; p < out.n_pairs; ++p)
|
||||
std::copy(Hs[p].begin(), Hs[p].end(), out.H.begin() + (size_t)p * n_fft);
|
||||
return out;
|
||||
}
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,34 @@
|
||||
#include "gpr/device.hpp"
|
||||
|
||||
#include <cstdio>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
void cu_check(CUresult r, const char* what) {
|
||||
if (r != CUDA_SUCCESS) {
|
||||
const char* s = nullptr;
|
||||
cuGetErrorString(r, &s);
|
||||
throw std::runtime_error(std::string("CUDA error in ") + what + ": " + (s ? s : "?"));
|
||||
}
|
||||
}
|
||||
|
||||
CudaContext::CudaContext() {
|
||||
cu_check(cuInit(0), "cuInit");
|
||||
cu_check(cuDeviceGet(&dev, 0), "cuDeviceGet");
|
||||
cu_check(cuDeviceGetAttribute(&cc_major, CU_DEVICE_ATTRIBUTE_COMPUTE_CAPABILITY_MAJOR, dev),
|
||||
"cc_major");
|
||||
cu_check(cuDeviceGetAttribute(&cc_minor, CU_DEVICE_ATTRIBUTE_COMPUTE_CAPABILITY_MINOR, dev),
|
||||
"cc_minor");
|
||||
cu_check(cuDevicePrimaryCtxRetain(&ctx, dev), "primaryCtxRetain");
|
||||
cu_check(cuCtxSetCurrent(ctx), "ctxSetCurrent");
|
||||
}
|
||||
|
||||
CudaContext::~CudaContext() {
|
||||
if (ctx) cuDevicePrimaryCtxRelease(dev);
|
||||
}
|
||||
|
||||
std::string CudaContext::arch_flag() const {
|
||||
return "--gpu-architecture=sm_" + std::to_string(cc_major) + std::to_string(cc_minor);
|
||||
}
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,104 @@
|
||||
#include "gpr/geometry.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace gpr {
|
||||
|
||||
double pair_geo_ref(const GeometryConfig& g, const BPConfig& bp, int i_tx, int i_rx, double yy) {
|
||||
double xc = (g.x_tx[i_tx] + g.x_rx[i_rx]) / 2.0;
|
||||
double Rtx = std::sqrt(std::pow(xc - g.x_tx[i_tx], 2) + std::pow(yy - g.y_tx[i_tx], 2) +
|
||||
std::pow(bp.comp_ref_depth - g.z_tx[i_tx], 2));
|
||||
double Rrx = std::sqrt(std::pow(xc - g.x_rx[i_rx], 2) + std::pow(yy - g.y_rx[i_rx], 2) +
|
||||
std::pow(bp.comp_ref_depth - g.z_rx[i_rx], 2));
|
||||
return 1.0 / (Rtx * Rrx + 1e-12) + 1e-30;
|
||||
}
|
||||
|
||||
std::vector<FrameOffset> frame_offsets(const std::vector<double>& frame_times, double t_ref,
|
||||
const MotionConfig& cfg) {
|
||||
std::vector<FrameOffset> out(frame_times.size());
|
||||
double pitch = cfg.pitch_deg * M_PI / 180.0;
|
||||
for (size_t k = 0; k < frame_times.size(); ++k) {
|
||||
double t = frame_times[k] - t_ref;
|
||||
double h = cfg.direction_sign * cfg.speed_m_s * t;
|
||||
out[k].dx_m = 0.0;
|
||||
out[k].dy_m = h * std::sin(pitch);
|
||||
out[k].dz_m = h * std::cos(pitch);
|
||||
out[k].dt_s = t;
|
||||
out[k].horizontal_m = h;
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
std::vector<double> frame_weights(const std::vector<FrameOffset>& offs, double edge_weight) {
|
||||
size_t n = offs.size();
|
||||
std::vector<double> w(n, 1.0);
|
||||
double edge = std::min(std::max(edge_weight, 0.0), 1.0);
|
||||
if (n == 0 || edge >= 1.0) return w;
|
||||
double dmax = 0.0;
|
||||
for (auto& o : offs) dmax = std::max(dmax, std::abs(o.dt_s));
|
||||
if (dmax <= 1e-12) return w;
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
double r = std::min(std::abs(offs[i].dt_s) / dmax, 1.0);
|
||||
w[i] = edge + (1.0 - edge) * 0.5 * (1.0 + std::cos(M_PI * r));
|
||||
}
|
||||
return w;
|
||||
}
|
||||
|
||||
std::array<double, 3> point_to_scene(double x, double y, double z, const MotionConfig& mot,
|
||||
const SARConfig& sar) {
|
||||
if (!sar.coord_mode_ground) return {x, y, z};
|
||||
double p = mot.pitch_deg * M_PI / 180.0;
|
||||
double ys = y * std::cos(p) - z * std::sin(p) + sar.ground_depth_m;
|
||||
double zs = y * std::sin(p) + z * std::cos(p);
|
||||
return {x, ys, zs};
|
||||
}
|
||||
|
||||
double ground_plane_y(double depth, double ground_depth_m, double pitch_rad) {
|
||||
return (-ground_depth_m + depth * std::sin(pitch_rad)) / std::cos(pitch_rad);
|
||||
}
|
||||
|
||||
Grid build_full_region_grid(const SARConfig& sar) {
|
||||
// numpy: linspace(lo, hi, max(round(span/step)+1, 2)); nearbyint = банковское
|
||||
// округление python round()
|
||||
auto axis = [](double lo, double hi, double step, std::vector<double>& out) {
|
||||
int n = std::max((int)std::nearbyint((hi - lo) / step) + 1, 2);
|
||||
out.resize(n);
|
||||
for (int i = 0; i < n; ++i) out[i] = lo + (hi - lo) * i / (double)(n - 1);
|
||||
};
|
||||
Grid g;
|
||||
axis(sar.x_min_m, sar.x_max_m, sar.voxel_m[0], g.x);
|
||||
axis(sar.y_min_m, sar.y_max_m, sar.voxel_m[1], g.y);
|
||||
axis(sar.z_min_m, sar.z_max_m, sar.voxel_m[2], g.z);
|
||||
return g;
|
||||
}
|
||||
|
||||
void notebook_grid_bounds(const GeometryConfig& geom, const BPConfig& bp, const SARConfig& sar,
|
||||
double pitch_deg, double x_margin_m,
|
||||
double& x_min, double& x_max, double& z_min, double& z_max) {
|
||||
double x_lo = geom.x_tx[0], x_hi = geom.x_tx[0];
|
||||
for (double x : geom.x_tx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
|
||||
for (double x : geom.x_rx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
|
||||
x_min = x_lo - x_margin_m;
|
||||
x_max = x_hi + x_margin_m;
|
||||
|
||||
// Радарная z линейна по Z сцены при фиксированной высоте:
|
||||
// z_rad(Y, Z=0) = -(Y - ground_depth)*sin(p), z = (depth - z_rad)/cos(p).
|
||||
double p = pitch_deg * M_PI / 180.0;
|
||||
double cos_p = std::cos(p), sin_p = std::sin(p);
|
||||
double z_lo = 0.0, z_hi = 0.0;
|
||||
bool first = true;
|
||||
for (double y : {sar.y_min_m, sar.y_max_m}) {
|
||||
for (double depth : {bp.min_depth, bp.max_depth}) {
|
||||
double z_rad0 = -(y - sar.ground_depth_m) * sin_p;
|
||||
double z = (depth - z_rad0) / cos_p;
|
||||
if (first) { z_lo = z_hi = z; first = false; }
|
||||
z_lo = std::min(z_lo, z);
|
||||
z_hi = std::max(z_hi, z);
|
||||
}
|
||||
}
|
||||
z_min = z_lo;
|
||||
z_max = z_hi;
|
||||
}
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,83 @@
|
||||
// Исходник CUDA-ядра для NVRTC (порт sar_frame_contribution из
|
||||
// Multi_antenna_3D_clean_v1.ipynb). Self-contained complex<float>.
|
||||
//
|
||||
// Геометрия (координаты, дальности, tau, вес компенсации) считается в double:
|
||||
// эталон ноутбука — float64, а в float32 позиция выборки A-скана (тысячи
|
||||
// отсчётов) теряет ~3e-4 отсчёта и расходится с эталоном на ~1e-4. A-сканы
|
||||
// остаются float32 — их значения O(1) и линейная интерполяция не теряет.
|
||||
#pragma once
|
||||
|
||||
namespace gpr {
|
||||
|
||||
inline const char* APERTURE_KERNEL_SRC = R"CUDA(
|
||||
struct cf { float x, y; };
|
||||
|
||||
// Взвешенный объём апертуры: один поток на воксель, внутренний цикл по
|
||||
// n_contrib = n_frames*n_pairs. Обрабатывается диапазон вокселей
|
||||
// [gid_begin, gid_end) — хост запускает ядро чанками, переиспользуя небольшие
|
||||
// acc/cnt-буферы (полный объём в VRAM не обязан помещаться: на десктопе с
|
||||
// занятой другими процессами картой свободных может быть <200 МБ).
|
||||
extern "C" __global__ void bp_aperture_contribution(
|
||||
const cf* __restrict__ H,
|
||||
const double t0, const double inv_dt, const int n_fft, const double inv_v,
|
||||
const double* __restrict__ xt, const double* __restrict__ yt, const double* __restrict__ zt,
|
||||
const double* __restrict__ xr, const double* __restrict__ yr, const double* __restrict__ zr,
|
||||
const double* __restrict__ inv_geo_ref, const float* __restrict__ wgt,
|
||||
const double* __restrict__ gx, const double* __restrict__ gy, const double* __restrict__ gz,
|
||||
const int NX, const int NY, const int NZ, const int n_contrib,
|
||||
const long gid_begin, const long gid_end,
|
||||
const double cos_p, const double sin_p, const double ground_depth,
|
||||
const double min_depth, const double max_depth,
|
||||
const double comp_pow, const double w_max, const int use_comp,
|
||||
cf* __restrict__ acc, float* __restrict__ cnt)
|
||||
{
|
||||
for (long gid = gid_begin + blockIdx.x * blockDim.x + threadIdx.x;
|
||||
gid < gid_end; gid += (long)gridDim.x * blockDim.x) {
|
||||
int ix = gid % NX;
|
||||
int iz = (gid / NX) % NZ;
|
||||
int iy = gid / ((long)NX * NZ);
|
||||
double x = gx[ix];
|
||||
double zs = gz[iz];
|
||||
// Сцена → оси радара: Y=0 сцены — земля, ground_depth ниже нижнего ряда
|
||||
// антенн (порт sar_scene_to_radar_yz).
|
||||
double y0 = gy[iy] - ground_depth;
|
||||
double y_rad = y0 * cos_p + zs * sin_p;
|
||||
double z_rad = -y0 * sin_p + zs * cos_p;
|
||||
bool depth_ok = (z_rad >= min_depth) && (z_rad <= max_depth);
|
||||
|
||||
double ax = 0.0, ay = 0.0;
|
||||
float c = 0.f;
|
||||
for (int i = 0; i < n_contrib; ++i) {
|
||||
double dxt = x - xt[i], dyt = y_rad - yt[i], dzt = z_rad - zt[i];
|
||||
double dxr = x - xr[i], dyr = y_rad - yr[i], dzr = z_rad - zr[i];
|
||||
double Rt = sqrt(dxt*dxt + dyt*dyt + dzt*dzt);
|
||||
double Rr = sqrt(dxr*dxr + dyr*dyr + dzr*dzr);
|
||||
double pos = ((Rt + Rr) * inv_v - t0) * inv_dt;
|
||||
bool inside = (pos >= 0.0) && (pos <= (double)(n_fft - 1));
|
||||
if (!inside || !depth_ok) continue;
|
||||
int k = (int)floor(pos);
|
||||
if (k < 0) k = 0;
|
||||
if (k > n_fft - 2) k = n_fft - 2;
|
||||
float f = (float)(pos - (double)k);
|
||||
const cf* h = H + (long)i * n_fft;
|
||||
float hx = h[k].x + f * (h[k+1].x - h[k].x);
|
||||
float hy = h[k].y + f * (h[k+1].y - h[k].y);
|
||||
double w = (double)wgt[i];
|
||||
if (use_comp) {
|
||||
double ratio = (1.0/(Rt*Rr + 1e-12) + 1e-30) * inv_geo_ref[i];
|
||||
double ww = 1.0/(pow(ratio, comp_pow) + 1e-12);
|
||||
ww = fmin(fmax(ww, 0.0), w_max);
|
||||
w *= ww;
|
||||
}
|
||||
ax += w * (double)hx;
|
||||
ay += w * (double)hy;
|
||||
c += wgt[i];
|
||||
}
|
||||
acc[gid - gid_begin].x = (float)ax;
|
||||
acc[gid - gid_begin].y = (float)ay;
|
||||
cnt[gid - gid_begin] = c;
|
||||
}
|
||||
}
|
||||
)CUDA";
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,48 @@
|
||||
#include "gpr/nvrtc_kernel.hpp"
|
||||
|
||||
#include <nvrtc.h>
|
||||
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/device.hpp"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
static void nvrtc_check(nvrtcResult r, const char* what) {
|
||||
if (r != NVRTC_SUCCESS)
|
||||
throw std::runtime_error(std::string("NVRTC error in ") + what + ": " +
|
||||
nvrtcGetErrorString(r));
|
||||
}
|
||||
|
||||
NvrtcKernel::~NvrtcKernel() {
|
||||
if (module_) cuModuleUnload(module_);
|
||||
}
|
||||
|
||||
void NvrtcKernel::compile(const char* src, const std::string& arch_flag, const char* func_name) {
|
||||
nvrtcProgram prog;
|
||||
nvrtc_check(nvrtcCreateProgram(&prog, src, "gpr_kernel.cu", 0, nullptr, nullptr),
|
||||
"createProgram");
|
||||
std::vector<const char*> opts{arch_flag.c_str()};
|
||||
nvrtcResult cr = nvrtcCompileProgram(prog, (int)opts.size(), opts.data());
|
||||
if (cr != NVRTC_SUCCESS) {
|
||||
size_t logsz = 0;
|
||||
nvrtcGetProgramLogSize(prog, &logsz);
|
||||
std::string log(logsz, '\0');
|
||||
if (logsz) nvrtcGetProgramLog(prog, &log[0]);
|
||||
nvrtcDestroyProgram(&prog);
|
||||
throw std::runtime_error("NVRTC compile failed:\n" + log);
|
||||
}
|
||||
// cubin под точную арх (без JIT PTX — драйвер грузит SASS напрямую)
|
||||
size_t cubsz = 0;
|
||||
nvrtc_check(nvrtcGetCUBINSize(prog, &cubsz), "getCUBINSize");
|
||||
std::string cubin(cubsz, '\0');
|
||||
nvrtc_check(nvrtcGetCUBIN(prog, &cubin[0]), "getCUBIN");
|
||||
nvrtcDestroyProgram(&prog);
|
||||
|
||||
cu_check(cuModuleLoadDataEx(&module_, cubin.data(), 0, nullptr, nullptr), "moduleLoad");
|
||||
cu_check(cuModuleGetFunction(&func_, module_, func_name), "getFunction");
|
||||
}
|
||||
|
||||
} // namespace gpr
|
||||
@@ -0,0 +1,233 @@
|
||||
#include "gpr/sar3d.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "kernel_src.hpp"
|
||||
|
||||
namespace gpr {
|
||||
|
||||
SarEngine::SarEngine(const GeometryConfig& g, const BPConfig& bp, const MotionConfig& mot,
|
||||
const SARConfig& sar)
|
||||
: geom_(g), bp_(bp), mot_(mot), sar_(sar) {
|
||||
geom_.finalize();
|
||||
ctx_ = std::make_unique<CudaContext>();
|
||||
kernel_ = std::make_unique<NvrtcKernel>();
|
||||
kernel_->compile(APERTURE_KERNEL_SRC, ctx_->arch_flag(), "bp_aperture_contribution");
|
||||
}
|
||||
|
||||
VolumeRaw SarEngine::aperture_volume(const std::vector<ApertureFrame>& frames, const Grid& grid) {
|
||||
if (frames.empty()) throw std::runtime_error("aperture_volume: нет кадров");
|
||||
auto pairs = geom_.pair_keys();
|
||||
int n_pairs = (int)pairs.size();
|
||||
int n_frames = (int)frames.size();
|
||||
int n_contrib = n_frames * n_pairs;
|
||||
|
||||
const AScanSet& a0 = *frames[0].ascans;
|
||||
int n_fft = a0.n_fft;
|
||||
double t0 = a0.t0, dt = a0.dt;
|
||||
|
||||
int NX = grid.NX(), NY = grid.NY(), NZ = grid.NZ();
|
||||
size_t nvox = (size_t)NX * NY * NZ;
|
||||
|
||||
double pitch = mot_.pitch_deg * M_PI / 180.0;
|
||||
// Опорное расхождение пары — на плоскости земли на глубине comp_ref_depth.
|
||||
double geo_ref_y = sar_.coord_mode_ground
|
||||
? ground_plane_y(bp_.comp_ref_depth, sar_.ground_depth_m, pitch)
|
||||
: 0.0;
|
||||
|
||||
// Плоские массивы по (кадр, пара)
|
||||
std::vector<c64> H_stack((size_t)n_contrib * n_fft);
|
||||
std::vector<double> xt(n_contrib), yt(n_contrib), zt(n_contrib);
|
||||
std::vector<double> xr(n_contrib), yr(n_contrib), zr(n_contrib);
|
||||
std::vector<double> igr(n_contrib);
|
||||
std::vector<float> wgt(n_contrib);
|
||||
int idx = 0;
|
||||
for (int f = 0; f < n_frames; ++f) {
|
||||
const AScanSet& a = *frames[f].ascans;
|
||||
if (a.n_fft != n_fft || std::abs(a.t0 - t0) > 1e-18)
|
||||
throw std::runtime_error("кадры апертуры: разная ось времени");
|
||||
double dx = frames[f].offset.dx_m, dy = frames[f].offset.dy_m, dz = frames[f].offset.dz_m;
|
||||
float w = (float)frames[f].weight;
|
||||
for (int p = 0; p < n_pairs; ++p) {
|
||||
int i = pairs[p].first, j = pairs[p].second;
|
||||
// A-скан пары p → в стек (каст c128→c64)
|
||||
const c128* src = a.pair_ptr(p);
|
||||
c64* dst = H_stack.data() + (size_t)idx * n_fft;
|
||||
for (int k = 0; k < n_fft; ++k) dst[k] = c64((float)src[k].real(), (float)src[k].imag());
|
||||
xt[idx] = geom_.x_tx[i] + dx; yt[idx] = geom_.y_tx[i] + dy;
|
||||
zt[idx] = geom_.z_tx[i] + dz;
|
||||
xr[idx] = geom_.x_rx[j] + dx; yr[idx] = geom_.y_rx[j] + dy;
|
||||
zr[idx] = geom_.z_rx[j] + dz;
|
||||
igr[idx] = 1.0 / pair_geo_ref(geom_, bp_, i, j, geo_ref_y);
|
||||
wgt[idx] = w;
|
||||
++idx;
|
||||
}
|
||||
}
|
||||
|
||||
double cos_p = 1.0, sin_p = 0.0, ground_depth = 0.0;
|
||||
if (sar_.coord_mode_ground) {
|
||||
cos_p = std::cos(pitch);
|
||||
sin_p = std::sin(pitch);
|
||||
ground_depth = sar_.ground_depth_m;
|
||||
}
|
||||
|
||||
// Заливка на устройство
|
||||
DeviceBuffer<c64> dH(H_stack.size()); dH.upload(H_stack.data(), H_stack.size());
|
||||
DeviceBuffer<double> dxt(n_contrib), dyt(n_contrib), dzt(n_contrib);
|
||||
DeviceBuffer<double> dxr(n_contrib), dyr(n_contrib), dzr(n_contrib);
|
||||
DeviceBuffer<double> digr(n_contrib);
|
||||
DeviceBuffer<float> dwgt(n_contrib);
|
||||
dxt.upload(xt.data(), n_contrib); dyt.upload(yt.data(), n_contrib); dzt.upload(zt.data(), n_contrib);
|
||||
dxr.upload(xr.data(), n_contrib); dyr.upload(yr.data(), n_contrib); dzr.upload(zr.data(), n_contrib);
|
||||
digr.upload(igr.data(), n_contrib); dwgt.upload(wgt.data(), n_contrib);
|
||||
DeviceBuffer<double> dgx(NX), dgy(NY), dgz(NZ);
|
||||
dgx.upload(grid.x.data(), NX); dgy.upload(grid.y.data(), NY); dgz.upload(grid.z.data(), NZ);
|
||||
|
||||
// Объём считается чанками с переиспользуемыми acc/cnt-буферами: полные
|
||||
// буферы (~115 МБ на сетку 9.6 млн вокселей) могут не влезать в остаток
|
||||
// VRAM, когда карту делят десктоп-процессы.
|
||||
const size_t chunk_vox = std::min<size_t>(nvox, 2'000'000);
|
||||
DeviceBuffer<c64> dacc(chunk_vox);
|
||||
DeviceBuffer<float> dcnt(chunk_vox);
|
||||
|
||||
double inv_dt = 1.0 / dt, inv_v = 1.0 / geom_.v;
|
||||
double min_d = bp_.min_depth, max_d = bp_.max_depth;
|
||||
double comp_pow = bp_.comp_range_power, w_max = bp_.comp_weight_max;
|
||||
int use_comp = sar_.use_compensation ? 1 : 0;
|
||||
|
||||
VolumeRaw out;
|
||||
out.NX = NX; out.NY = NY; out.NZ = NZ;
|
||||
out.acc.resize(nvox); out.cnt.resize(nvox);
|
||||
int block = 256;
|
||||
for (size_t begin = 0; begin < nvox; begin += chunk_vox) {
|
||||
const size_t end = std::min(nvox, begin + chunk_vox);
|
||||
long gid_begin = (long)begin, gid_end = (long)end;
|
||||
void* args[] = {
|
||||
dH.addr(), &t0, &inv_dt, &n_fft, &inv_v,
|
||||
dxt.addr(), dyt.addr(), dzt.addr(), dxr.addr(), dyr.addr(), dzr.addr(),
|
||||
digr.addr(), dwgt.addr(), dgx.addr(), dgy.addr(), dgz.addr(),
|
||||
&NX, &NY, &NZ, &n_contrib, &gid_begin, &gid_end,
|
||||
&cos_p, &sin_p, &ground_depth, &min_d, &max_d,
|
||||
&comp_pow, &w_max, &use_comp, dacc.addr(), dcnt.addr()};
|
||||
int gridDim = (int)std::min<long>(65535, ((long)(end - begin) + block - 1) / block);
|
||||
cu_check(cuLaunchKernel(kernel_->func(), gridDim, 1, 1, block, 1, 1, 0, 0, args, nullptr),
|
||||
"launch aperture");
|
||||
cu_check(cuCtxSynchronize(), "sync");
|
||||
dacc.download(out.acc.data() + begin, end - begin);
|
||||
dcnt.download(out.cnt.data() + begin, end - begin);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
// scipy.ndimage.gaussian_filter: ядро радиуса int(truncate*sigma+0.5), reflect.
|
||||
std::vector<double> gaussian_kernel_1d(double sigma, int radius) {
|
||||
std::vector<double> w(2 * radius + 1);
|
||||
double sum = 0.0;
|
||||
for (int i = -radius; i <= radius; ++i) {
|
||||
w[i + radius] = std::exp(-0.5 * i * i / (sigma * sigma));
|
||||
sum += w[i + radius];
|
||||
}
|
||||
for (auto& v : w) v /= sum;
|
||||
return w;
|
||||
}
|
||||
|
||||
// Корреляция вдоль оси с reflect-границами ('d c b a | a b c d').
|
||||
inline int reflect_index(int i, int n) {
|
||||
while (i < 0 || i >= n) {
|
||||
if (i < 0) i = -i - 1;
|
||||
if (i >= n) i = 2 * n - i - 1;
|
||||
}
|
||||
return i;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
FinishedVolume finish_volume(const VolumeRaw& raw, const Grid& grid, double pitch_deg,
|
||||
const BPConfig& bp, const SARConfig& sar) {
|
||||
int NX = raw.NX, NY = raw.NY, NZ = raw.NZ;
|
||||
size_t nvox = (size_t)NX * NY * NZ;
|
||||
FinishedVolume out;
|
||||
out.NX = NX; out.NY = NY; out.NZ = NZ;
|
||||
|
||||
std::vector<double> vol(nvox);
|
||||
for (size_t i = 0; i < nvox; ++i)
|
||||
vol[i] = std::abs(c128(raw.acc[i].real(), raw.acc[i].imag())) / ((double)raw.cnt[i] + 1e-12);
|
||||
|
||||
// Маска глубины в радарных осях (в ядре невалидные вкладов не получали,
|
||||
// но |0/(0+eps)| уже 0; обнуление здесь повторяет ноутбук буквально).
|
||||
if (sar.coord_mode_ground) {
|
||||
double p = pitch_deg * M_PI / 180.0;
|
||||
double cos_p = std::cos(p), sin_p = std::sin(p);
|
||||
for (int iy = 0; iy < NY; ++iy) {
|
||||
double y0 = grid.y[iy] - sar.ground_depth_m;
|
||||
for (int iz = 0; iz < NZ; ++iz) {
|
||||
double z_rad = -y0 * sin_p + grid.z[iz] * cos_p;
|
||||
if (z_rad >= bp.min_depth && z_rad <= bp.max_depth) continue;
|
||||
double* row = vol.data() + ((size_t)iy * NZ + iz) * NX;
|
||||
std::fill(row, row + NX, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Плоскости Y для нормировки: без верхних/нижних norm_edge_margin_m.
|
||||
std::vector<bool> ref_plane(NY, true);
|
||||
double margin = sar.norm_edge_margin_m;
|
||||
double y_span = grid.y.back() - grid.y.front();
|
||||
if (margin > 0.0 && y_span > 2.5 * margin) {
|
||||
for (int iy = 0; iy < NY; ++iy)
|
||||
ref_plane[iy] = grid.y[iy] >= grid.y.front() + margin &&
|
||||
grid.y[iy] <= grid.y.back() - margin;
|
||||
}
|
||||
auto ref_max = [&](const std::vector<double>& v) {
|
||||
double m = 0.0;
|
||||
for (int iy = 0; iy < NY; ++iy) {
|
||||
if (!ref_plane[iy]) continue;
|
||||
const double* plane = v.data() + (size_t)iy * NZ * NX;
|
||||
for (size_t i = 0; i < (size_t)NZ * NX; ++i) m = std::max(m, plane[i]);
|
||||
}
|
||||
return m + 1e-12;
|
||||
};
|
||||
|
||||
double level_raw = ref_max(vol);
|
||||
for (auto& v : vol) v /= level_raw;
|
||||
|
||||
// Гаусс sigma=(0, s, s): сглаживание по Z и X внутри каждого Y-плана
|
||||
// (порядок осей как у scipy: сначала ось 1, затем ось 2).
|
||||
if (sar.smooth_sigma > 0.0) {
|
||||
int radius = (int)(4.0 * sar.smooth_sigma + 0.5);
|
||||
const auto kernel = gaussian_kernel_1d(sar.smooth_sigma, radius);
|
||||
std::vector<double> tmp((size_t)NZ * NX);
|
||||
for (int iy = 0; iy < NY; ++iy) {
|
||||
double* plane = vol.data() + (size_t)iy * NZ * NX;
|
||||
// вдоль Z (ось 1)
|
||||
for (int iz = 0; iz < NZ; ++iz)
|
||||
for (int ix = 0; ix < NX; ++ix) {
|
||||
double s = 0.0;
|
||||
for (int k = -radius; k <= radius; ++k)
|
||||
s += kernel[k + radius] * plane[(size_t)reflect_index(iz + k, NZ) * NX + ix];
|
||||
tmp[(size_t)iz * NX + ix] = s;
|
||||
}
|
||||
// вдоль X (ось 2)
|
||||
for (int iz = 0; iz < NZ; ++iz)
|
||||
for (int ix = 0; ix < NX; ++ix) {
|
||||
double s = 0.0;
|
||||
for (int k = -radius; k <= radius; ++k)
|
||||
s += kernel[k + radius] * tmp[(size_t)iz * NX + reflect_index(ix + k, NX)];
|
||||
plane[(size_t)iz * NX + ix] = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double level_smooth = ref_max(vol);
|
||||
out.volume.resize(nvox);
|
||||
for (size_t i = 0; i < nvox; ++i)
|
||||
out.volume[i] = (float)std::max(vol[i] / level_smooth, 0.0);
|
||||
out.level = level_raw * level_smooth;
|
||||
return out;
|
||||
}
|
||||
|
||||
} // namespace gpr
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,48 @@
|
||||
# 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.
|
||||
@@ -0,0 +1,55 @@
|
||||
#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_;
|
||||
};
|
||||
@@ -0,0 +1,147 @@
|
||||
#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
|
||||
+4
-7
@@ -4,7 +4,6 @@
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class TtyProtocolWriter {
|
||||
public:
|
||||
@@ -22,12 +21,9 @@ public:
|
||||
TtyProtocolWriter(TtyProtocolWriter&& other) noexcept = delete;
|
||||
TtyProtocolWriter& operator=(TtyProtocolWriter&& other) noexcept = delete;
|
||||
|
||||
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);
|
||||
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);
|
||||
StatsSnapshot stats() const;
|
||||
|
||||
const std::string& path() const;
|
||||
@@ -35,6 +31,7 @@ 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_;
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,450 @@
|
||||
#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
|
||||
@@ -1,55 +0,0 @@
|
||||
#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
|
||||
@@ -1,255 +0,0 @@
|
||||
#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
@@ -0,0 +1,51 @@
|
||||
#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
@@ -0,0 +1,21 @@
|
||||
#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
@@ -0,0 +1,91 @@
|
||||
#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
|
||||
@@ -0,0 +1,63 @@
|
||||
#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
@@ -0,0 +1,448 @@
|
||||
#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
|
||||
@@ -0,0 +1,76 @@
|
||||
#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
|
||||
+13
-14
@@ -2,27 +2,21 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include <vector>
|
||||
|
||||
#include "channel_bundle_support.hpp"
|
||||
#include "calibration_master.hpp"
|
||||
#include "reference_master.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
|
||||
@@ -32,14 +26,19 @@ 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);
|
||||
|
||||
void preprocess_collection(ipc::RawSweepCollection& collection) const;
|
||||
[[nodiscard]] auto preprocess_collection(const ipc::RawSweepCollection& raw_collection) const
|
||||
-> ipc::PreprocessedCollection;
|
||||
|
||||
const config::RunConfig& config_;
|
||||
S21CalibrationBundle calibration_;
|
||||
S21ReferenceBundle reference_;
|
||||
S21ObjectCalibrationBundle object_calibration_;
|
||||
CalibrationMaster& calibration_master_;
|
||||
ReferenceMaster& reference_master_;
|
||||
ipc::ShmRing& raw_ring_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing* preprocessed_tap_ring_ = nullptr;
|
||||
|
||||
@@ -1,25 +1,26 @@
|
||||
#include "data_preprocessor.hpp"
|
||||
#include "runtime_wait.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <numbers>
|
||||
#include <iostream>
|
||||
#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();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
auto build_notch_mask(
|
||||
[[nodiscard]] auto build_notch_mask(
|
||||
std::span<const float> frequency_hz,
|
||||
const config::PreprocessNotchConfig& notch
|
||||
) -> std::vector<float> {
|
||||
@@ -50,16 +51,15 @@ auto build_notch_mask(
|
||||
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 = std::numbers::pi * (frequency - trans_low_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 + std::cos(arg)));
|
||||
const double arg = kPi * (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 = std::numbers::pi * (frequency - high_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 - std::cos(arg)));
|
||||
const double arg = kPi * (frequency - high_hz) / taper_width_hz;
|
||||
mask[index] *= static_cast<float>(0.5 * (1.0 + std::cos(arg)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -67,8 +67,6 @@ auto build_notch_mask(
|
||||
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) {
|
||||
@@ -77,72 +75,127 @@ 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) {
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
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 runtime::FatalPipelineError("Preprocessed ring slot is too small for serialized collection");
|
||||
throw std::runtime_error("Preprocessed ring slot is too small for serialized collection");
|
||||
}
|
||||
if (preprocessed_tap_ring_ != nullptr && !preprocessed_tap_ring_->push(serialized_preprocessed)) {
|
||||
throw runtime::FatalPipelineError("Preprocessed tap ring slot is too small for serialized collection");
|
||||
throw std::runtime_error("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{};
|
||||
runtime::CollectionErrors errors;
|
||||
ipc::RawSweepCollection raw_collection{};
|
||||
std::uint64_t error_count = 0;
|
||||
std::uint64_t consecutive_errors = 0;
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
try {
|
||||
if (!raw_ring_.pop(serialized_raw)) {
|
||||
(void)config::wait_or_stop(config_.runtime.idle_sleep_ms, stop_requested);
|
||||
if (!try_pop_raw_collection(serialized_raw, &raw_collection)) {
|
||||
continue;
|
||||
}
|
||||
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;
|
||||
const auto preprocessed_collection = preprocess_collection(raw_collection);
|
||||
publish_preprocessed_collection(preprocessed_collection);
|
||||
consecutive_errors = 0; // made progress; reset the escalation counter
|
||||
} catch (const std::exception& exc) {
|
||||
errors.report("data_preprocessor", exc);
|
||||
(void)config::wait_or_stop(std::max(1U, config_.runtime.idle_sleep_ms), stop_requested);
|
||||
// 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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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");
|
||||
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));
|
||||
}
|
||||
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();
|
||||
|
||||
return preprocessed;
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
|
||||
@@ -1,14 +1,49 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include "calibration_master.hpp"
|
||||
#include "data_preprocessor.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
#include "reference_master.hpp"
|
||||
#include "run_config.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 = radar::runtime::initialize(argc, argv);
|
||||
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,
|
||||
@@ -20,22 +55,37 @@ int main(int argc, char** argv) {
|
||||
config.rings.preprocessed.capacity,
|
||||
config.rings.preprocessed.slot_size_bytes
|
||||
);
|
||||
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
|
||||
);
|
||||
}
|
||||
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);
|
||||
|
||||
radar::preprocessing::DataPreprocessor preprocessor(
|
||||
config,
|
||||
calibration_master,
|
||||
reference_master,
|
||||
raw_ring,
|
||||
preprocessed_ring,
|
||||
preprocessed_tap_ring ? &*preprocessed_tap_ring : nullptr
|
||||
&preprocessed_tap_ring
|
||||
);
|
||||
preprocessor.run(radar::runtime::stop_requested);
|
||||
preprocessor.run(g_stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_preprocessor error: " << exception.what() << '\n';
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
#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
|
||||
@@ -0,0 +1,213 @@
|
||||
#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,7 +2,6 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@@ -38,21 +37,23 @@ 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;
|
||||
|
||||
// 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>;
|
||||
// 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;
|
||||
|
||||
void publish_locator(const ipc::ResultCollection& collection);
|
||||
// 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);
|
||||
|
||||
const config::RunConfig& config_;
|
||||
config::RunConfig processing_config_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing& results_ring_;
|
||||
ProcessorRegistry processors_{};
|
||||
|
||||
+87
-33
@@ -2,52 +2,83 @@
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "run_config.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
struct HistoryTarget {
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t capture_start_ns = 0;
|
||||
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 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;
|
||||
// Absent geometry overrides use the initial run_config geometry.
|
||||
std::optional<config::GprConfig> gpr{};
|
||||
LegacyGprMode legacy_gpr_mode = LegacyGprMode::Point;
|
||||
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;
|
||||
// BP object-detection stop level, as a fraction of the global peak; peaks
|
||||
// below it are not objects.
|
||||
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.
|
||||
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).
|
||||
// correction) or "int_focus" (focusing residual only). Mirrors the Python
|
||||
// Horns_motion_3libre.py MOTION_CORRECTION_MODE selector.
|
||||
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;
|
||||
// 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;
|
||||
// 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;
|
||||
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
|
||||
@@ -59,39 +90,63 @@ 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.
|
||||
// 3D SAR (sar3d processor) parameters; defaults mirror the reference
|
||||
// notebook Multi_antenna_3D_clean_v1.ipynb (cells 0/4). Frequency band,
|
||||
// depth gate, antenna selection and motion (speed, look angle, direction,
|
||||
// phase correction) are shared with the gpr_* fields.
|
||||
std::uint32_t sar3d_frame_radius = 0U;
|
||||
// SAR_VOXEL_M: the voxel steps along X, Y and Z.
|
||||
float sar3d_edge_frame_weight = 0.8F;
|
||||
// Frame pitch for ground coordinates and inter-frame offsets. Override
|
||||
// disabled -> the pitch follows gpr_look_angle_deg (notebook
|
||||
// BP_GROUND_PITCH_DEG = None).
|
||||
bool sar3d_pitch_override_enabled = false;
|
||||
float sar3d_pitch_deg = 17.5F;
|
||||
// Height of the LOWER antenna row above the ground; scene Y=0 is the ground.
|
||||
float sar3d_ground_depth_m = 1.25F;
|
||||
// Scene grid bounds (metres): X across, Y height above the ground, Z along
|
||||
// the ground away from the radar. Voxels whose radar depth falls outside
|
||||
// [gpr_min_depth, gpr_max_depth] are zeroed by the mask regardless.
|
||||
// Doubles, not floats: the parity tool feeds the notebook-derived bounds
|
||||
// through here and a float roundtrip shifts the grid by ~1e-7 m, flipping
|
||||
// knife-edge voxels of the depth mask against the reference volumes.
|
||||
double sar3d_x_min_m = -2.0;
|
||||
double sar3d_x_max_m = 2.0;
|
||||
double sar3d_y_min_m = -0.5;
|
||||
double sar3d_y_max_m = 2.0;
|
||||
double sar3d_z_min_m = 0.0;
|
||||
double sar3d_z_max_m = 8.0;
|
||||
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;
|
||||
// Volume finalization: normalization ignores this much of the grid's Y
|
||||
// edges (constant-range ridge), then Gaussian smoothing over (Z, X).
|
||||
float sar3d_norm_edge_margin_m = 0.3F;
|
||||
float sar3d_smooth_sigma = 1.5F;
|
||||
// Geometric spreading compensation 1/(Rtx*Rrx)^power, capped.
|
||||
float sar3d_comp_range_power = 0.3F;
|
||||
// Fallback frame segmentation when per-trace capture stamps are absent.
|
||||
std::uint32_t sar3d_tx_per_segment = 2U;
|
||||
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
|
||||
bool sar3d_normalize_pairs = true;
|
||||
// Debug: when non-empty, the processor dumps every finished volume to this
|
||||
// directory (raw float32 + json header). Used by the parity tool only.
|
||||
std::string sar3d_volume_dump_dir{};
|
||||
// Bottom-view projection angle around the vertical Y axis, degrees:
|
||||
// 0 = front view (X horizontal), 90 = side view (Z horizontal),
|
||||
// intermediate values project along the rotated horizontal direction.
|
||||
float sar3d_bottom_view_deg = 0.0F;
|
||||
// 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;
|
||||
// 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;
|
||||
std::uint64_t history_command_seq = 0;
|
||||
HistoryCommand history_command = HistoryCommand::None;
|
||||
};
|
||||
|
||||
class ProcessingLiveConfigLoader {
|
||||
public:
|
||||
ProcessingLiveConfigLoader(std::string path, const config::RunConfig& run_config);
|
||||
explicit ProcessingLiveConfigLoader(std::string path);
|
||||
|
||||
[[nodiscard]] auto current() const -> ProcessingLiveConfig;
|
||||
[[nodiscard]] auto refresh_if_needed() -> ProcessingLiveConfig;
|
||||
@@ -101,7 +156,6 @@ 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,6 +1,4 @@
|
||||
#include "data_processor.hpp"
|
||||
#include "runtime_wait.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
@@ -9,6 +7,7 @@
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "bscan_processor.hpp"
|
||||
@@ -22,26 +21,16 @@ namespace {
|
||||
constexpr const char* kDefaultProcessorMode = "pass_through";
|
||||
constexpr std::size_t kBscanReplayWindow = 50U;
|
||||
|
||||
[[nodiscard]] auto processing_history_limit(const config::RunConfig& config, const ProcessingLiveConfig& live) -> std::size_t {
|
||||
[[nodiscard]] auto replay_history_limit(const config::RunConfig& config) -> 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));
|
||||
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}));
|
||||
return std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow});
|
||||
}
|
||||
|
||||
void publish_result_collection(
|
||||
const ipc::ResultCollection& collection, ipc::ShmRing& results_ring, std::uint64_t& oversized_results
|
||||
) {
|
||||
void publish_result_collection(const ipc::ResultCollection& collection, ipc::ShmRing& results_ring) {
|
||||
const auto out_bytes = ipc::serialize_result_collection(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";
|
||||
if (!results_ring.push(out_bytes)) {
|
||||
throw std::runtime_error("Results ring slot is too small for serialized collection");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -55,12 +44,11 @@ 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, config),
|
||||
live_config_loader_(config.runtime.processing_live_config_path),
|
||||
locator_server_(locator_server) {
|
||||
if (processors_.empty()) {
|
||||
throw std::runtime_error("DataProcessor requires at least one processor");
|
||||
@@ -73,123 +61,120 @@ 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();
|
||||
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::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).
|
||||
std::optional<double> last_reprocessed_socket_speed = std::nullopt;
|
||||
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
try {
|
||||
// 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_config_raw = live_config_loader_.refresh_if_needed();
|
||||
const auto live_config = resolve_effective_live_config(live_config_raw);
|
||||
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;
|
||||
|
||||
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;
|
||||
});
|
||||
};
|
||||
// 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;
|
||||
|
||||
if (live_revision != last_replayed_revision || socket_speed_dirty) {
|
||||
// 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.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;
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
} 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);
|
||||
if (!preprocessed.capture_start_ns || preprocessed.capture_end_ns < preprocessed.capture_start_ns) {
|
||||
throw std::runtime_error("Preprocessed collection requires a valid acquisition window");
|
||||
preprocessed_history.push_back(std::move(preprocessed));
|
||||
while (preprocessed_history.size() > history_limit) {
|
||||
preprocessed_history.erase(preprocessed_history.begin());
|
||||
}
|
||||
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(
|
||||
current,
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.begin(), earlier),
|
||||
preprocessed_history.back(),
|
||||
std::span<const ipc::PreprocessedCollection>(preprocessed_history.data(), preprocessed_history.size() - 1U),
|
||||
processor,
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(result_collection, results_ring_, oversized_results);
|
||||
if (!fed_back) {
|
||||
publish_locator(result_collection);
|
||||
}
|
||||
errors.progress();
|
||||
publish_result_collection(result_collection, results_ring_);
|
||||
publish_locator(result_collection, live_config);
|
||||
consecutive_errors = 0; // made progress; reset the escalation counter
|
||||
continue;
|
||||
}
|
||||
|
||||
(void)config::wait_or_stop(config_.runtime.idle_sleep_ms, stop_requested);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.runtime.idle_sleep_ms));
|
||||
} catch (const std::exception& exc) {
|
||||
errors.report("data_processor", exc);
|
||||
(void)config::wait_or_stop(std::max(1U, config_.runtime.idle_sleep_ms), stop_requested);
|
||||
// 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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -201,27 +186,19 @@ auto DataProcessor::process_collection(
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
const auto started_at = std::chrono::steady_clock::now();
|
||||
auto result = processor.process_collection(processing_config_, preprocessed, previous_collections, live_config);
|
||||
auto result = processor.process_collection(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::requested_mode(const ProcessingLiveConfig& live_config) const -> const std::string& {
|
||||
return live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface& {
|
||||
if (auto found = processors_.find(requested_mode(live_config)); found != processors_.end()) {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
|
||||
if (auto found = processors_.find(requested_mode); found != processors_.end()) {
|
||||
return *(found->second);
|
||||
}
|
||||
if (auto fallback = processors_.find(default_processor_mode_); fallback != processors_.end()) {
|
||||
@@ -231,28 +208,59 @@ auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -
|
||||
}
|
||||
|
||||
auto DataProcessor::should_replay_entire_history(const ProcessingLiveConfig& live_config) const -> bool {
|
||||
return requested_mode(live_config) == "bscan";
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
return requested_mode == "bscan";
|
||||
}
|
||||
|
||||
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> {
|
||||
auto DataProcessor::resolve_effective_live_config(const ProcessingLiveConfig& live_config) const
|
||||
-> ProcessingLiveConfig {
|
||||
if (live_config.ignore_socket_speed || locator_server_ == nullptr) {
|
||||
return std::nullopt;
|
||||
return live_config;
|
||||
}
|
||||
return locator_server_->latest_socket_speed();
|
||||
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;
|
||||
}
|
||||
|
||||
void DataProcessor::publish_locator(
|
||||
const ipc::ResultCollection& collection
|
||||
const ipc::ResultCollection& collection,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) {
|
||||
if (locator_server_ == nullptr) {
|
||||
if (locator_server_ == nullptr || !locator_server_->is_running()) {
|
||||
return;
|
||||
}
|
||||
locator_server_->publish(collection);
|
||||
locator_server_->publish(collection, build_locator_filter(live_config));
|
||||
}
|
||||
|
||||
auto create_default_processors() -> ProcessorRegistry {
|
||||
@@ -269,6 +277,10 @@ auto create_default_processors() -> ProcessorRegistry {
|
||||
auto processor = std::make_unique<GprProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
{
|
||||
auto processor = std::make_unique<LegacyGprProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
{
|
||||
auto processor = std::make_unique<Sar3dProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
|
||||
@@ -1,14 +1,43 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "data_processor.hpp"
|
||||
#include "locator/tcp_server.hpp"
|
||||
#include "pipeline_runtime.hpp"
|
||||
#include "run_config.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) {
|
||||
@@ -27,8 +56,11 @@ namespace {
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
install_signal_handlers();
|
||||
|
||||
try {
|
||||
const auto config = radar::runtime::initialize(argc, argv);
|
||||
const auto config_path = read_config_path(argc, argv);
|
||||
const auto config = radar::config::load_run_config(config_path);
|
||||
|
||||
auto preprocessed_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.preprocessed.name,
|
||||
@@ -50,7 +82,7 @@ int main(int argc, char** argv) {
|
||||
radar::processing::create_default_processors(),
|
||||
locator_server.get()
|
||||
);
|
||||
processor.run(radar::runtime::stop_requested);
|
||||
processor.run(g_stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_processor error: " << exception.what() << '\n';
|
||||
|
||||
+317
-132
@@ -9,7 +9,6 @@
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
@@ -19,6 +18,41 @@ 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;
|
||||
@@ -33,55 +67,41 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error(field_name + " must be one of: int_minus, int_focus");
|
||||
}
|
||||
|
||||
[[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");
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void assign_optional(const Json& root, const char* key, T& target) {
|
||||
const auto found = root.find(key);
|
||||
if (found == root.end()) {
|
||||
void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) {
|
||||
if (value == "backprojection") {
|
||||
return;
|
||||
}
|
||||
const auto field_name = std::string("processing.") + key;
|
||||
if constexpr (std::is_same_v<T, float>) {
|
||||
target = parse_f32(*found, field_name);
|
||||
if (value == "legacy_point") {
|
||||
config.legacy_gpr_mode = LegacyGprMode::Point;
|
||||
} else if (value == "legacy_extended") {
|
||||
config.legacy_gpr_mode = LegacyGprMode::Extended;
|
||||
} 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(
|
||||
"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";
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_s_parameter_channel(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "s21" || value == "s11") {
|
||||
return value;
|
||||
}
|
||||
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 (!std::isfinite(numeric) || numeric < 0.0 || numeric >= static_cast<double>(std::numeric_limits<std::uint64_t>::max())) {
|
||||
if (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 (numeric != rounded) {
|
||||
if (std::fabs(numeric - rounded) > 1e-6) {
|
||||
throw std::runtime_error(field_name + " must be integer");
|
||||
}
|
||||
return static_cast<std::uint64_t>(rounded);
|
||||
@@ -108,31 +128,7 @@ void assign_optional(const Json& root, const char* key, T& target) {
|
||||
return result;
|
||||
}
|
||||
|
||||
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 {
|
||||
[[nodiscard]] auto parse_live_config(const std::string& json_text, const std::string& path) -> ProcessingLiveConfig {
|
||||
if (json_text.empty()) {
|
||||
throw std::runtime_error("Processing live config is empty: " + path);
|
||||
}
|
||||
@@ -149,40 +145,128 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
|
||||
}
|
||||
|
||||
ProcessingLiveConfig config{};
|
||||
if (const auto found = root.find("settings_revision"); found != root.end()) {
|
||||
config.settings_revision = parse_u64_number(*found, "processing.settings_revision");
|
||||
}
|
||||
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);
|
||||
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");
|
||||
}
|
||||
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.processor_mode = found->get<std::string>();
|
||||
}
|
||||
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");
|
||||
}
|
||||
config::validate_gpr_config(*config.gpr, run_config);
|
||||
config.pass_through_channel =
|
||||
parse_s_parameter_channel(found->get<std::string>(), "processing.pass_through_channel");
|
||||
}
|
||||
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);
|
||||
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");
|
||||
}
|
||||
config.pass_through_fixed_y_enabled = found->get<bool>();
|
||||
}
|
||||
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, "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");
|
||||
}
|
||||
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_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>());
|
||||
}
|
||||
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");
|
||||
@@ -195,25 +279,100 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
|
||||
}
|
||||
config.gpr_motion_mode = parse_gpr_motion_mode(found->get<std::string>(), "processing.gpr_motion_mode");
|
||||
}
|
||||
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_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>();
|
||||
}
|
||||
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");
|
||||
}
|
||||
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);
|
||||
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>());
|
||||
}
|
||||
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},
|
||||
}) {
|
||||
assign_optional(root, key, *target);
|
||||
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>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_max_detected_objects_to_draw"); found != root.end()) {
|
||||
config.gpr_max_detected_objects_to_draw =
|
||||
@@ -231,58 +390,85 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
|
||||
config.sar3d_frame_radius = parse_u32_number(*found, "processing.sar3d_frame_radius");
|
||||
}
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"sar3d_edge_frame_weight", &config.sar3d_edge_frame_weight},
|
||||
std::pair{"sar3d_pitch_deg", &config.sar3d_pitch_deg},
|
||||
std::pair{"sar3d_ground_depth_m", &config.sar3d_ground_depth_m},
|
||||
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_norm_edge_margin_m", &config.sar3d_norm_edge_margin_m},
|
||||
std::pair{"sar3d_smooth_sigma", &config.sar3d_smooth_sigma},
|
||||
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},
|
||||
std::pair{"sar3d_bottom_view_deg", &config.sar3d_bottom_view_deg},
|
||||
}) {
|
||||
assign_optional(root, key, *target);
|
||||
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>());
|
||||
}
|
||||
}
|
||||
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},
|
||||
std::pair{"sar3d_x_min_m", &config.sar3d_x_min_m},
|
||||
std::pair{"sar3d_x_max_m", &config.sar3d_x_max_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_min_m", &config.sar3d_z_min_m},
|
||||
std::pair{"sar3d_z_max_m", &config.sar3d_z_max_m},
|
||||
}) {
|
||||
assign_optional(root, key, *target);
|
||||
}
|
||||
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");
|
||||
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");
|
||||
}
|
||||
config.history_removed.push_back(target);
|
||||
*target = found->get<double>();
|
||||
}
|
||||
}
|
||||
if (const auto found = root.find("sar3d_tx_per_segment"); found != root.end()) {
|
||||
config.sar3d_tx_per_segment = parse_u32_number(*found, "processing.sar3d_tx_per_segment");
|
||||
if (config.sar3d_tx_per_segment == 0U) {
|
||||
throw std::runtime_error("processing.sar3d_tx_per_segment must be >= 1");
|
||||
}
|
||||
}
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"sar3d_pitch_override_enabled", &config.sar3d_pitch_override_enabled},
|
||||
std::pair{"sar3d_use_compensation", &config.sar3d_use_compensation},
|
||||
std::pair{"sar3d_normalize_pairs", &config.sar3d_normalize_pairs},
|
||||
}) {
|
||||
if (const auto found = root.find(key); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error(std::string("processing.") + key + " must be bool");
|
||||
}
|
||||
*target = found->get<bool>();
|
||||
}
|
||||
}
|
||||
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("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>();
|
||||
}
|
||||
if (const auto found = root.find("history_command_seq"); found != root.end()) {
|
||||
config.history_command_seq = parse_u64_number(*found, "processing.history_command_seq");
|
||||
}
|
||||
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");
|
||||
}
|
||||
config.history_command = parse_history_command(found->get<std::string>());
|
||||
}
|
||||
return config;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
ProcessingLiveConfigLoader::ProcessingLiveConfigLoader(std::string path, const config::RunConfig& run_config)
|
||||
ProcessingLiveConfigLoader::ProcessingLiveConfigLoader(std::string path)
|
||||
: 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");
|
||||
@@ -315,10 +501,9 @@ auto ProcessingLiveConfigLoader::refresh_if_needed() -> ProcessingLiveConfig {
|
||||
return current_;
|
||||
}
|
||||
|
||||
// Remember rejected content too; retry only when the file actually changes.
|
||||
current_ = parse_live_config(json_text, path_);
|
||||
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,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -9,9 +10,47 @@
|
||||
|
||||
namespace radar::locator {
|
||||
|
||||
// Extract the final coherent-GPR objects, dropping malformed or non-finite rows.
|
||||
[[nodiscard]] auto observations_from_collection(const ipc::ResultCollection& collection)
|
||||
-> std::vector<Observation>;
|
||||
// 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>;
|
||||
|
||||
// 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,6 +36,7 @@ class ClientQueue {
|
||||
// Wake any waiter and reject further pushes. Idempotent.
|
||||
void close();
|
||||
|
||||
[[nodiscard]] auto is_closed() const -> bool;
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_{};
|
||||
@@ -87,7 +88,6 @@ 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);
|
||||
void publish(const ipc::ResultCollection& collection, const FilterParams& filter);
|
||||
|
||||
// Most recent `vlc` value received from any connected client, or nullopt
|
||||
// if no client has ever submitted one since startup. Lock-free.
|
||||
@@ -148,7 +148,11 @@ 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(bool wait_for_lock = true);
|
||||
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 cache_latest_packet(std::vector<std::uint8_t> packet);
|
||||
[[nodiscard]] auto latest_packet_copy() const -> std::optional<std::vector<std::uint8_t>>;
|
||||
|
||||
|
||||
@@ -6,9 +6,7 @@
|
||||
#include <cstring>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
@@ -19,12 +17,14 @@ 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,8 +35,11 @@ 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 nullptr;
|
||||
return fallback;
|
||||
}
|
||||
|
||||
// Quantise to one decimal place (0.1 m = decimetres). Equivalent to Python's
|
||||
@@ -46,6 +49,29 @@ 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 {
|
||||
@@ -70,6 +96,17 @@ 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));
|
||||
@@ -79,20 +116,66 @@ void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t v
|
||||
|
||||
} // namespace
|
||||
|
||||
auto observations_from_collection(const ipc::ResultCollection& collection) -> std::vector<Observation> {
|
||||
auto observations_from_collection(
|
||||
const ipc::ResultCollection& collection,
|
||||
const FilterParams& filter
|
||||
) -> std::vector<Observation> {
|
||||
const auto* payload = find_object_table(collection);
|
||||
if (payload == nullptr) {
|
||||
return {};
|
||||
}
|
||||
const std::size_t columns = payload->table_columns;
|
||||
std::vector<Observation> result;
|
||||
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)});
|
||||
|
||||
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);
|
||||
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),
|
||||
});
|
||||
}
|
||||
return result;
|
||||
}
|
||||
@@ -117,6 +200,15 @@ 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();
|
||||
}
|
||||
|
||||
@@ -124,9 +216,6 @@ 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,6 +197,11 @@ 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(
|
||||
@@ -220,16 +225,10 @@ void ClientSession::start(
|
||||
std::atomic<double>& shared_vlc_slot,
|
||||
std::atomic<std::int64_t>& 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;
|
||||
}
|
||||
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);
|
||||
});
|
||||
}
|
||||
|
||||
void ClientSession::enqueue(std::vector<std::uint8_t> packet) {
|
||||
@@ -279,7 +278,14 @@ void ClientSession::writer_loop() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
finish_loop();
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void ClientSession::reader_loop(
|
||||
@@ -289,58 +295,51 @@ void ClientSession::reader_loop(
|
||||
std::array<std::uint8_t, kPacketHeaderSize> header_buffer{};
|
||||
std::vector<std::uint8_t> payload_buffer;
|
||||
|
||||
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;
|
||||
}
|
||||
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) && 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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
} 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();
|
||||
// A session is reapable after both threads have stopped using its socket.
|
||||
// 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.
|
||||
if (loops_finished_.fetch_add(1, std::memory_order_acq_rel) + 1U == 2U) {
|
||||
exited_.store(true, std::memory_order_release);
|
||||
}
|
||||
@@ -418,13 +417,7 @@ void TcpServer::start() {
|
||||
}
|
||||
|
||||
listen_fd_ = fd;
|
||||
try {
|
||||
acceptor_thread_ = std::thread([this]() { acceptor_loop(); });
|
||||
} catch (...) {
|
||||
shutdown_and_close(listen_fd_);
|
||||
running_.store(false, std::memory_order_release);
|
||||
throw;
|
||||
}
|
||||
acceptor_thread_ = std::thread([this]() { acceptor_loop(); });
|
||||
}
|
||||
|
||||
void TcpServer::stop() {
|
||||
@@ -434,14 +427,13 @@ 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;
|
||||
{
|
||||
@@ -461,12 +453,12 @@ auto TcpServer::is_running() const -> bool {
|
||||
return running_.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
void TcpServer::publish(const ipc::ResultCollection& collection) {
|
||||
void TcpServer::publish(const ipc::ResultCollection& collection, const FilterParams& filter) {
|
||||
if (!running_.load(std::memory_order_acquire)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto observations = observations_from_collection(collection);
|
||||
const auto observations = observations_from_collection(collection, filter);
|
||||
const auto payload_json = build_payload_json(observations, config_.protocol_version);
|
||||
auto packet = encode_packet(payload_json, config_.device_id);
|
||||
cache_latest_packet(packet);
|
||||
@@ -474,7 +466,7 @@ void TcpServer::publish(const ipc::ResultCollection& collection) {
|
||||
// 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.
|
||||
reap_finished_clients(false);
|
||||
try_reap_finished_clients();
|
||||
}
|
||||
|
||||
auto TcpServer::latest_socket_speed() const -> std::optional<double> {
|
||||
@@ -533,27 +525,19 @@ void TcpServer::acceptor_loop() {
|
||||
|
||||
reap_finished_clients();
|
||||
|
||||
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());
|
||||
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);
|
||||
}
|
||||
session->start(latest_socket_speed_, latest_socket_speed_at_ns_);
|
||||
enroll_client(std::move(session));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -569,13 +553,35 @@ void TcpServer::broadcast_packet(const std::vector<std::uint8_t>& packet) {
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::reap_finished_clients(bool wait_for_lock) {
|
||||
void TcpServer::reap_finished_clients() {
|
||||
std::vector<std::unique_ptr<ClientSession>> to_join;
|
||||
{
|
||||
std::unique_lock<std::mutex> guard(clients_mutex_, std::defer_lock);
|
||||
if (wait_for_lock) {
|
||||
guard.lock();
|
||||
} else if (!guard.try_lock()) {
|
||||
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()) {
|
||||
return;
|
||||
}
|
||||
auto first_dead = std::partition(
|
||||
|
||||
@@ -21,4 +21,15 @@ 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,7 +10,8 @@
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
// Temporal "approach" filter for coherent-BP detections.
|
||||
// Temporal "approach" filter for coherent-BP detections — a port of the objects-only
|
||||
// filter in Horns_motion_3libre's demonstrate notebook.
|
||||
//
|
||||
// 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
|
||||
@@ -29,8 +30,12 @@ namespace radar::processing {
|
||||
// inter-frame gap (`kMaxFrameGapSeconds`), so a stale history from a previous run cannot
|
||||
// spuriously confirm objects.
|
||||
//
|
||||
// Reprocessing replaces the same (collection_id, capture_start_ns) entry. Earlier
|
||||
// capture times rebuild the history when the processor replays its retained window.
|
||||
// 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.
|
||||
class ObjectApproachFilter {
|
||||
public:
|
||||
struct Point {
|
||||
@@ -38,16 +43,20 @@ class ObjectApproachFilter {
|
||||
double z_m;
|
||||
};
|
||||
|
||||
// Acquisition time drives both ordering and motion intervals. min_frames <= 1 disables filtering.
|
||||
// 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.
|
||||
[[nodiscard]] auto confirm(
|
||||
const std::vector<Point>& objects,
|
||||
std::uint64_t frame_id,
|
||||
std::uint64_t capture_start_ns,
|
||||
double frame_time_seconds,
|
||||
double speed_m_s,
|
||||
double look_angle_deg,
|
||||
std::size_t min_frames
|
||||
) -> std::vector<bool> {
|
||||
record_frame(Frame{frame_id, capture_start_ns, objects});
|
||||
record_frame(Frame{frame_id, frame_time_seconds, objects});
|
||||
const std::size_t history_depth = std::max<std::size_t>(min_frames, 1U);
|
||||
while (history_.size() > history_depth) {
|
||||
history_.pop_front();
|
||||
@@ -70,14 +79,17 @@ class ObjectApproachFilter {
|
||||
private:
|
||||
struct Frame {
|
||||
std::uint64_t id;
|
||||
std::uint64_t capture_start_ns;
|
||||
double time_seconds;
|
||||
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.capture_start_ns > history_.back().capture_start_ns) {
|
||||
if (history_.empty() || frame.id > history_.back().id) {
|
||||
history_.push_back(std::move(frame));
|
||||
} else if (frame.id == history_.back().id && frame.capture_start_ns == history_.back().capture_start_ns) {
|
||||
} else if (frame.id == history_.back().id) {
|
||||
history_.back() = std::move(frame);
|
||||
} else {
|
||||
history_.clear();
|
||||
@@ -85,7 +97,7 @@ class ObjectApproachFilter {
|
||||
}
|
||||
}
|
||||
|
||||
// Fixed matching tolerances. Range decreases as the
|
||||
// Fixed matching tolerances (Horns_motion notebook 0.2 block). 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;
|
||||
@@ -110,10 +122,8 @@ 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 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) {
|
||||
const double dt = history_[earlier_index + 1U].time_seconds - earlier.time_seconds;
|
||||
if (!(dt > 0.0) || dt > kMaxFrameGapSeconds) {
|
||||
return false; // non-monotonic time, or a gap that breaks the track
|
||||
}
|
||||
const double expected_range_shift = range_step_per_second * dt;
|
||||
|
||||
@@ -8,13 +8,9 @@
|
||||
|
||||
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).
|
||||
// 3D SAR over a synthesized aperture, computed by the external gpr_cpp
|
||||
// CUDA/NVRTC library (VNA_AlGo). Emits 2D max-projections of the volume
|
||||
// (side XZ and top XY views) as ImageF32 collection payloads.
|
||||
class Sar3dProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
Sar3dProcessor();
|
||||
@@ -29,6 +25,8 @@ class Sar3dProcessor final : public ProcessorInterface {
|
||||
) -> ipc::ResultCollection override;
|
||||
|
||||
private:
|
||||
// Pimpl keeps gpr/CUDA headers (cuda.h, nvrtc.h) out of the shared
|
||||
// processor include path; only sar3d_processor.cpp needs them.
|
||||
struct State;
|
||||
std::unique_ptr<State> state_;
|
||||
};
|
||||
|
||||
@@ -1,248 +0,0 @@
|
||||
#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,17 +93,42 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
return std::abs(sample);
|
||||
}
|
||||
|
||||
// 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 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;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto compute_bscan_profile(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> BScanProfile {
|
||||
const auto selected_samples = std::span<const ipc::Complex32>(trace.s21);
|
||||
const auto selected_samples = selected_trace_samples(trace, live_config);
|
||||
const std::size_t point_count = std::min(trace.frequency_hz.size(), selected_samples.size());
|
||||
if (point_count < 2U) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
const double configured_start_hz = static_cast<double>(live_config.bscan_start_freq_mhz) * 1'000'000.0;
|
||||
@@ -127,35 +152,35 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
}
|
||||
|
||||
if (filtered_freq_hz.size() < 2U) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
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 {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(filtered_freq_hz.front() / df));
|
||||
if (start_bin < 0) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
const auto start_index = static_cast<std::size_t>(start_bin);
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() / 2U)) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() - filtered_count + 1U)) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
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 {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
if (start_index > fft_len || filtered_count > (fft_len - start_index)) {
|
||||
return {};
|
||||
return fallback_profile(trace, selected_samples);
|
||||
}
|
||||
|
||||
std::vector<std::complex<double>> spectrum(fft_len, std::complex<double>(0.0, 0.0));
|
||||
@@ -212,6 +237,8 @@ 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) {
|
||||
|
||||
+398
-80
@@ -1,4 +1,5 @@
|
||||
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;
|
||||
@@ -6,8 +7,13 @@ 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
|
||||
@@ -45,12 +51,19 @@ constexpr double kScoreCfEps = 1e-12;
|
||||
|
||||
using PairKey = std::uint64_t;
|
||||
|
||||
// Frequency-domain speed correction mode:
|
||||
// Frequency-domain speed correction mode (Horns_motion_3libre.py MOTION_CONFIG):
|
||||
// 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.
|
||||
// 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.
|
||||
std::vector<std::uint32_t> input_positions{};
|
||||
std::vector<std::uint32_t> output_positions{};
|
||||
std::vector<double> x_tx{};
|
||||
@@ -63,6 +76,11 @@ 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;
|
||||
@@ -250,6 +268,20 @@ 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.
|
||||
@@ -282,7 +314,7 @@ void normalize_in_place(std::vector<double>& values) {
|
||||
return;
|
||||
}
|
||||
for (auto& value : values) {
|
||||
value /= maximum + 1e-12;
|
||||
value /= maximum;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -513,10 +545,67 @@ 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 bp::Background& background_mean
|
||||
const std::unordered_map<PairKey, std::vector<std::complex<double>>>& background_mean
|
||||
) -> std::vector<SelectedTrace> {
|
||||
std::vector<SelectedTrace> traces{};
|
||||
traces.reserve(collection.traces.size());
|
||||
@@ -548,7 +637,15 @@ void validate_collection_trace_order(
|
||||
throw std::runtime_error("GPR requires unique selected combos; duplicate combo: " + combo_to_string(trace.combo));
|
||||
}
|
||||
|
||||
selected.s21 = bp::subtract_background(trace, background_mean);
|
||||
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);
|
||||
}
|
||||
|
||||
traces.push_back(std::move(selected));
|
||||
}
|
||||
@@ -556,6 +653,114 @@ 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,
|
||||
@@ -601,7 +806,7 @@ void validate_collection_trace_order(
|
||||
return result;
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::nearbyint(frequency_hz.front() / df_hz));
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(frequency_hz.front() / df_hz));
|
||||
if (start_bin < 0) {
|
||||
return result;
|
||||
}
|
||||
@@ -639,6 +844,50 @@ void validate_collection_trace_order(
|
||||
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));
|
||||
}
|
||||
@@ -647,7 +896,7 @@ void validate_collection_trace_order(
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
double ground_depth_m, double pitch_rad
|
||||
double imaging_plane_y_m
|
||||
) -> GridDefinition {
|
||||
GridDefinition grid{};
|
||||
if (selection.x_tx.empty() || selection.x_rx.empty() || !(max_depth_m > min_z_m)) {
|
||||
@@ -675,14 +924,14 @@ void validate_collection_trace_order(
|
||||
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],
|
||||
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_tx[tx_index],
|
||||
imaging_plane_y_m - 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],
|
||||
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
z_value - selection.z_rx[rx_index]
|
||||
);
|
||||
}
|
||||
@@ -699,10 +948,10 @@ void validate_collection_trace_order(
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
double ground_depth_m, double pitch_rad
|
||||
double imaging_plane_y_m
|
||||
) -> std::vector<double> {
|
||||
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);
|
||||
std::vector<double> signature;
|
||||
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 3U);
|
||||
const auto append = [&](const std::vector<double>& axis) {
|
||||
signature.insert(signature.end(), axis.begin(), axis.end());
|
||||
};
|
||||
@@ -714,8 +963,7 @@ void validate_collection_trace_order(
|
||||
append(selection.z_rx);
|
||||
signature.push_back(max_depth_m);
|
||||
signature.push_back(min_z_m);
|
||||
signature.push_back(ground_depth_m);
|
||||
signature.push_back(pitch_rad);
|
||||
signature.push_back(imaging_plane_y_m);
|
||||
return signature;
|
||||
}
|
||||
|
||||
@@ -737,18 +985,67 @@ void validate_collection_trace_order(
|
||||
return (ascan.samples[index] * (1.0 - frac)) + (ascan.samples[index + 1U] * frac);
|
||||
}
|
||||
|
||||
// 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
|
||||
[[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
|
||||
) -> double {
|
||||
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;
|
||||
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);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto backproject_coherent(
|
||||
@@ -756,11 +1053,12 @@ void validate_collection_trace_order(
|
||||
const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
|
||||
const GridDefinition& grid,
|
||||
const GeometrySelection& selection,
|
||||
double ground_depth_m, double pitch_rad,
|
||||
double imaging_plane_y_m,
|
||||
double velocity_mps,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
double range_power
|
||||
double range_power,
|
||||
double angle_power
|
||||
) -> BpMap {
|
||||
const std::size_t width = grid.x_grid.size();
|
||||
const std::size_t height = grid.z_grid.size();
|
||||
@@ -780,6 +1078,9 @@ void validate_collection_trace_order(
|
||||
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;
|
||||
@@ -790,17 +1091,20 @@ void validate_collection_trace_order(
|
||||
if (ascan_it == ascans_by_pair.end() || ascan_it->second.time_s.empty()) {
|
||||
continue;
|
||||
}
|
||||
const double geo_ref = attenuation_geo_at_ref_depth(
|
||||
const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth(
|
||||
trace.tx_local_index,
|
||||
trace.rx_local_index,
|
||||
selection,
|
||||
ground_depth_m, pitch_rad
|
||||
imaging_plane_y_m
|
||||
);
|
||||
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],
|
||||
});
|
||||
}
|
||||
|
||||
@@ -831,9 +1135,16 @@ void validate_collection_trace_order(
|
||||
}
|
||||
|
||||
const auto sample = interpolate_complex(ascan, tau_s);
|
||||
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);
|
||||
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
|
||||
);
|
||||
coherent_sum += sample * weight;
|
||||
incoherent_sum += std::abs(sample) * weight;
|
||||
contribution_count += 1.0;
|
||||
@@ -842,8 +1153,8 @@ void validate_collection_trace_order(
|
||||
if (!(contribution_count > 0.0)) {
|
||||
continue;
|
||||
}
|
||||
coherent_sum /= contribution_count + 1e-12;
|
||||
incoherent_sum /= contribution_count + 1e-12;
|
||||
coherent_sum /= contribution_count;
|
||||
incoherent_sum /= contribution_count;
|
||||
result.coherent[cell_index] = coherent_sum;
|
||||
result.incoherent[cell_index] = incoherent_sum;
|
||||
result.image[cell_index] = std::abs(coherent_sum);
|
||||
@@ -1194,7 +1505,7 @@ void apply_depth_gate(
|
||||
const ObjectRecord& object,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
double ground_depth_m, double pitch_rad
|
||||
double imaging_plane_y_m
|
||||
) -> std::vector<double> {
|
||||
std::vector<double> signature{};
|
||||
signature.reserve(selected_traces.size());
|
||||
@@ -1203,12 +1514,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],
|
||||
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_tx[tx],
|
||||
imaging_plane_y_m - selection.y_tx[tx],
|
||||
object.z_m - selection.z_tx[tx]
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
object.x_m - selection.x_rx[rx],
|
||||
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_rx[rx],
|
||||
imaging_plane_y_m - selection.y_rx[rx],
|
||||
object.z_m - selection.z_rx[rx]
|
||||
);
|
||||
signature.push_back(0.5 * (r_tx + r_rx));
|
||||
@@ -1220,12 +1531,12 @@ void mark_sidelobe_candidates(
|
||||
std::vector<ObjectRecord>& objects,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
double ground_depth_m, double pitch_rad
|
||||
double imaging_plane_y_m
|
||||
) {
|
||||
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, ground_depth_m, pitch_rad));
|
||||
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, imaging_plane_y_m));
|
||||
}
|
||||
|
||||
for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
|
||||
@@ -1429,7 +1740,7 @@ void add_bp_score_metrics(
|
||||
}
|
||||
}
|
||||
|
||||
// Select the FINAL visible objects here, so the
|
||||
// Select the FINAL visible objects exactly as Horns_motion_3libre.py does, 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);
|
||||
@@ -1533,6 +1844,8 @@ 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()) {
|
||||
@@ -1540,15 +1853,16 @@ void add_bp_score_metrics(
|
||||
}
|
||||
|
||||
validate_collection_trace_order(run_config, collection);
|
||||
const auto background_mean = bp::background_mean(previous_collections, live_config.gpr_background_subtract_enabled,
|
||||
live_config.gpr_background_mean_count);
|
||||
const auto background_mean = build_background_mean(previous_collections, selection, live_config);
|
||||
auto selected_traces = collect_selected_traces(collection, selection, background_mean);
|
||||
if (selected_traces.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
// Coherent BP propagates in air, as in Horns_motion_4x4_ground.py.
|
||||
const double velocity_mps = bp::kWaveSpeed;
|
||||
// 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;
|
||||
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);
|
||||
@@ -1557,12 +1871,36 @@ void add_bp_score_metrics(
|
||||
return results;
|
||||
}
|
||||
|
||||
const auto timing = bp::collection_timing(collection);
|
||||
const double motion_look_angle_deg = live_config.gpr_look_angle_deg;
|
||||
// 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);
|
||||
for (auto& trace : selected_traces) {
|
||||
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);
|
||||
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
|
||||
);
|
||||
}
|
||||
|
||||
std::unordered_map<PairKey, AscanResult> ascans_by_pair{};
|
||||
@@ -1579,30 +1917,9 @@ void add_bp_score_metrics(
|
||||
if (ascans_by_pair.empty()) {
|
||||
return results;
|
||||
}
|
||||
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;
|
||||
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);
|
||||
|
||||
// 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
|
||||
@@ -1611,9 +1928,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, ground_depth_m, pitch_rad);
|
||||
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
if (cached_grid.x_grid.empty() || signature != cached_grid_signature) {
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, ground_depth_m, pitch_rad);
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
cached_grid_signature = signature;
|
||||
}
|
||||
const GridDefinition& grid = cached_grid;
|
||||
@@ -1626,11 +1943,12 @@ void add_bp_score_metrics(
|
||||
ascans_by_pair,
|
||||
grid,
|
||||
selection,
|
||||
ground_depth_m, pitch_rad,
|
||||
imaging_plane_y_m,
|
||||
velocity_mps,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
std::max(0.0, static_cast<double>(live_config.gpr_range_comp_power))
|
||||
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)
|
||||
);
|
||||
if (bp.image.empty()) {
|
||||
return results;
|
||||
@@ -1643,7 +1961,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, ground_depth_m, pitch_rad);
|
||||
mark_sidelobe_candidates(objects, selected_traces, selection, imaging_plane_y_m);
|
||||
add_bp_score_metrics(objects, live_config);
|
||||
|
||||
if (live_config.gpr_remove_sidelobe_objects_enabled) {
|
||||
@@ -1662,7 +1980,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.
|
||||
// objects confirmed as a >= min_frames motion-consistent track (Horns_motion notebook).
|
||||
const auto visible = select_visible_objects(objects, live_config);
|
||||
std::vector<ObjectApproachFilter::Point> visible_points{};
|
||||
visible_points.reserve(visible.size());
|
||||
@@ -1672,7 +1990,7 @@ void add_bp_score_metrics(
|
||||
const auto confirmed = approach_filter.confirm(
|
||||
visible_points,
|
||||
collection.collection_id,
|
||||
collection.capture_start_ns,
|
||||
static_cast<double>(collection.monotonic_ns) * 1e-9,
|
||||
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,5 +1,4 @@
|
||||
#include "gpr_processor.hpp"
|
||||
#include "backprojection.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
@@ -23,6 +22,7 @@ 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,4 +39,17 @@ 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,9 +10,11 @@ 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) {
|
||||
@@ -20,7 +22,11 @@ auto PassThroughProcessor::process_collection(
|
||||
payload.processing_name = name();
|
||||
payload.kind = ipc::ResultKind::TraceComplex;
|
||||
payload.frequency_hz = trace.frequency_hz;
|
||||
payload.trace = trace.s21;
|
||||
if (live_config.pass_through_channel == "s11") {
|
||||
payload.trace = trace.s11;
|
||||
} else {
|
||||
payload.trace = trace.s21;
|
||||
}
|
||||
|
||||
ipc::ResultBlock block{};
|
||||
block.combo = trace.combo;
|
||||
|
||||
@@ -1,558 +0,0 @@
|
||||
#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
|
||||
@@ -1,91 +0,0 @@
|
||||
#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
|
||||
@@ -1,733 +0,0 @@
|
||||
#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
|
||||
@@ -1,125 +0,0 @@
|
||||
#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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,349 @@
|
||||
// Parity check of the sar3d processor against the reference notebook
|
||||
// Multi_antenna_3D_clean_v1.ipynb.
|
||||
//
|
||||
// Feeds a preprocessed snapshot of an extracted replay dataset through
|
||||
// Sar3dProcessor exactly the way the live pipeline would (geometry and combo
|
||||
// order from the dataset's config_profile.json, per-trace capture windows from
|
||||
// the snapshot's meta.json) and dumps the finished volume for comparison with
|
||||
// the notebook's *_volume.npz export:
|
||||
//
|
||||
// sar3d_parity <dataset_root> <center_snapshot> [--dump-dir <dir>]
|
||||
// python_app/scripts/sar3d_volume_compare.py <dir>/sar3d_volume_<id> <ref.npz>
|
||||
//
|
||||
// <dataset_root> contains config_profile.json and preprocessed/<snapshot>/.
|
||||
// Defaults mirror the parameters the reference volumes were built with
|
||||
// (f 2..6 GHz, depth 2.5..8 m, speed 1.45 m/s, look angle 17.5 deg).
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <dirent.h>
|
||||
#include <fstream>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/geometry.hpp"
|
||||
#include "processing_live_config.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "sar3d_processor.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
// Minimal numpy .npy loader (v1.0, 1-D complex128/complex64/float64/float32).
|
||||
struct NpyArray {
|
||||
std::vector<double> f64{};
|
||||
std::vector<std::complex<double>> c128{};
|
||||
};
|
||||
|
||||
NpyArray load_npy(const std::string& path) {
|
||||
std::ifstream in(path, std::ios::binary);
|
||||
if (!in) {
|
||||
throw std::runtime_error("cannot open " + path);
|
||||
}
|
||||
char magic[6];
|
||||
in.read(magic, 6);
|
||||
std::uint8_t version[2];
|
||||
in.read(reinterpret_cast<char*>(version), 2);
|
||||
std::uint16_t header_len = 0;
|
||||
in.read(reinterpret_cast<char*>(&header_len), 2);
|
||||
std::string header(header_len, '\0');
|
||||
in.read(header.data(), header_len);
|
||||
const auto shape_pos = header.find("'shape':");
|
||||
const auto left = header.find('(', shape_pos);
|
||||
const auto right = header.find(')', shape_pos);
|
||||
const long count = std::stol(header.substr(left + 1, right - left - 1));
|
||||
|
||||
NpyArray array{};
|
||||
const auto has = [&header](const char* token) { return header.find(token) != std::string::npos; };
|
||||
if (has("c8")) {
|
||||
array.c128.resize(count);
|
||||
std::vector<std::complex<float>> tmp(count);
|
||||
in.read(reinterpret_cast<char*>(tmp.data()), count * sizeof(std::complex<float>));
|
||||
for (long i = 0; i < count; ++i) {
|
||||
array.c128[i] = std::complex<double>(tmp[i].real(), tmp[i].imag());
|
||||
}
|
||||
} else if (has("c16")) {
|
||||
array.c128.resize(count);
|
||||
in.read(reinterpret_cast<char*>(array.c128.data()), count * sizeof(std::complex<double>));
|
||||
} else if (has("f4")) {
|
||||
array.f64.resize(count);
|
||||
std::vector<float> tmp(count);
|
||||
in.read(reinterpret_cast<char*>(tmp.data()), count * sizeof(float));
|
||||
for (long i = 0; i < count; ++i) {
|
||||
array.f64[i] = tmp[i];
|
||||
}
|
||||
} else {
|
||||
array.f64.resize(count);
|
||||
in.read(reinterpret_cast<char*>(array.f64.data()), count * sizeof(double));
|
||||
}
|
||||
return array;
|
||||
}
|
||||
|
||||
// Live config matching the reference notebook run (npz meta parameters).
|
||||
// The default program grid is a fixed window (X -2..2, Z 0..8), while the
|
||||
// notebook derives its bounds from the antenna span and the depth gate — so
|
||||
// the notebook bounds are recomputed here and passed explicitly.
|
||||
auto build_live_config(const radar::config::RunConfig& run_config)
|
||||
-> radar::processing::ProcessingLiveConfig {
|
||||
radar::processing::ProcessingLiveConfig live{};
|
||||
live.processor_mode = "sar3d";
|
||||
live.gpr_start_freq_mhz = 2000.0F;
|
||||
live.gpr_stop_freq_mhz = 6000.0F;
|
||||
live.gpr_min_depth_m = 2.5F;
|
||||
live.gpr_max_depth_m = 8.0F;
|
||||
live.gpr_speed_m_s = 1.45F;
|
||||
live.gpr_look_angle_deg = 17.5F;
|
||||
live.gpr_direction_sign = 1.0F;
|
||||
live.gpr_apply_freq_phase_correction = true;
|
||||
|
||||
gpr::GeometryConfig geom{};
|
||||
geom.input_idx.clear();
|
||||
geom.output_idx.clear();
|
||||
geom.rx_positions.clear();
|
||||
geom.tx_positions.clear();
|
||||
for (const auto& entry : run_config.gpr.rx_geometry) {
|
||||
geom.input_idx.push_back(static_cast<int>(entry.input_pos));
|
||||
geom.rx_positions.push_back({entry.x_m, entry.y_m, entry.z_m});
|
||||
}
|
||||
for (const auto& entry : run_config.gpr.tx_geometry) {
|
||||
geom.output_idx.push_back(static_cast<int>(entry.output_pos));
|
||||
geom.tx_positions.push_back({entry.x_m, entry.y_m, entry.z_m});
|
||||
}
|
||||
geom.finalize();
|
||||
gpr::BPConfig bp{};
|
||||
bp.min_depth = live.gpr_min_depth_m;
|
||||
bp.max_depth = live.gpr_max_depth_m;
|
||||
gpr::SARConfig sar{};
|
||||
double x_min = 0.0, x_max = 0.0, z_min = 0.0, z_max = 0.0;
|
||||
gpr::notebook_grid_bounds(geom, bp, sar, live.gpr_look_angle_deg, /*x_margin_m=*/2.0,
|
||||
x_min, x_max, z_min, z_max);
|
||||
live.sar3d_x_min_m = x_min;
|
||||
live.sar3d_x_max_m = x_max;
|
||||
live.sar3d_z_min_m = z_min;
|
||||
live.sar3d_z_max_m = z_max;
|
||||
return live;
|
||||
}
|
||||
|
||||
auto load_collection(
|
||||
const radar::config::RunConfig& run_config,
|
||||
const std::string& root,
|
||||
const std::string& snapshot,
|
||||
std::uint64_t collection_id
|
||||
) -> radar::ipc::PreprocessedCollection {
|
||||
radar::ipc::PreprocessedCollection collection{};
|
||||
collection.collection_id = collection_id;
|
||||
|
||||
// Per-trace capture windows from the snapshot's meta.json; the segmented
|
||||
// timing model is reconstructed from them exactly like in the notebook.
|
||||
std::map<std::pair<std::uint32_t, std::uint32_t>, std::pair<std::uint64_t, std::uint64_t>> windows{};
|
||||
{
|
||||
std::ifstream in(root + "/preprocessed/" + snapshot + "/meta.json");
|
||||
if (in) {
|
||||
nlohmann::json meta{};
|
||||
in >> meta;
|
||||
for (const auto& trace : meta.value("traces", nlohmann::json::array())) {
|
||||
if (!trace.contains("capture_start_ns") || !trace.contains("capture_end_ns")) {
|
||||
continue;
|
||||
}
|
||||
windows[{trace.at("input").get<std::uint32_t>(), trace.at("output").get<std::uint32_t>()}] = {
|
||||
trace.at("capture_start_ns").get<std::uint64_t>(),
|
||||
trace.at("capture_end_ns").get<std::uint64_t>(),
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::uint64_t first = 0U;
|
||||
std::uint64_t last = 0U;
|
||||
for (const auto& [key, window] : windows) {
|
||||
if (first == 0U || window.first < first) {
|
||||
first = window.first;
|
||||
}
|
||||
last = std::max(last, window.second);
|
||||
}
|
||||
if (first == 0U) {
|
||||
// No meta labels: fall back to the `_ns<...>` folder suffix.
|
||||
const auto ns_pos = snapshot.rfind("_ns");
|
||||
if (ns_pos == std::string::npos) {
|
||||
throw std::runtime_error("snapshot has no meta.json and no _ns suffix: " + snapshot);
|
||||
}
|
||||
last = static_cast<std::uint64_t>(std::stoull(snapshot.substr(ns_pos + 3)));
|
||||
first = last;
|
||||
}
|
||||
collection.capture_start_ns = first;
|
||||
collection.capture_end_ns = last;
|
||||
collection.monotonic_ns = (first + last) / 2U;
|
||||
|
||||
for (const auto& combo : run_config.run_combos) {
|
||||
char path[512];
|
||||
std::snprintf(path, sizeof(path), "%s/preprocessed/%s/i%u_o%u_s21.npy",
|
||||
root.c_str(), snapshot.c_str(), combo.input_pos, combo.output_pos);
|
||||
const auto s21 = load_npy(path);
|
||||
std::snprintf(path, sizeof(path), "%s/preprocessed/%s/i%u_o%u_freq.npy",
|
||||
root.c_str(), snapshot.c_str(), combo.input_pos, combo.output_pos);
|
||||
const auto freq = load_npy(path);
|
||||
|
||||
radar::ipc::SweepTraceBlock trace{};
|
||||
trace.combo = combo;
|
||||
trace.frequency_hz.assign(freq.f64.begin(), freq.f64.end());
|
||||
trace.s21.reserve(s21.c128.size());
|
||||
for (const auto& sample : s21.c128) {
|
||||
trace.s21.push_back(radar::ipc::Complex32{
|
||||
.re = static_cast<float>(sample.real()),
|
||||
.im = static_cast<float>(sample.imag()),
|
||||
});
|
||||
}
|
||||
trace.s11.resize(trace.s21.size());
|
||||
if (const auto found = windows.find({combo.input_pos, combo.output_pos}); found != windows.end()) {
|
||||
trace.capture_start_ns = found->second.first;
|
||||
trace.capture_end_ns = found->second.second;
|
||||
}
|
||||
collection.traces.push_back(std::move(trace));
|
||||
}
|
||||
return collection;
|
||||
}
|
||||
|
||||
void print_projection_argmax(
|
||||
const radar::ipc::ResultPayload& payload,
|
||||
const char* col_axis_name,
|
||||
const char* row_axis_name
|
||||
) {
|
||||
std::size_t argmax = 0U;
|
||||
float best = -1.0F;
|
||||
for (std::size_t index = 0U; index < payload.image_values.size(); ++index) {
|
||||
if (payload.image_values[index] > best) {
|
||||
best = payload.image_values[index];
|
||||
argmax = index;
|
||||
}
|
||||
}
|
||||
const auto nx = payload.image_x_axis.size();
|
||||
const auto col = argmax % nx;
|
||||
const auto row = argmax / nx;
|
||||
std::printf(
|
||||
"%s: max=%.6f at %s=%.1f cm, %s=%.1f cm\n",
|
||||
payload.processing_name.c_str(),
|
||||
static_cast<double>(best),
|
||||
col_axis_name,
|
||||
static_cast<double>(payload.image_x_axis[col]) * 100.0,
|
||||
row_axis_name,
|
||||
static_cast<double>(payload.image_y_axis[row]) * 100.0
|
||||
);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 3) {
|
||||
std::printf("usage: %s <dataset_root> <center_snapshot> [--dump-dir <dir>]\n", argv[0]);
|
||||
return 1;
|
||||
}
|
||||
const std::string root = argv[1];
|
||||
const std::string center = argv[2];
|
||||
std::string dump_dir{};
|
||||
for (int index = 3; index + 1 < argc; ++index) {
|
||||
if (std::string{argv[index]} == "--dump-dir") {
|
||||
dump_dir = argv[index + 1];
|
||||
}
|
||||
}
|
||||
|
||||
const std::string preprocessed = root + "/preprocessed";
|
||||
std::vector<std::string> snapshots{};
|
||||
if (DIR* dir = opendir(preprocessed.c_str())) {
|
||||
for (dirent* entry = nullptr; (entry = readdir(dir)) != nullptr;) {
|
||||
const std::string name = entry->d_name;
|
||||
if (!name.empty() && (std::isdigit(name[0]) != 0)) {
|
||||
snapshots.push_back(name);
|
||||
}
|
||||
}
|
||||
closedir(dir);
|
||||
}
|
||||
std::sort(snapshots.begin(), snapshots.end());
|
||||
const auto center_it = std::find(snapshots.begin(), snapshots.end(), center);
|
||||
if (center_it == snapshots.end()) {
|
||||
std::printf("center %s not found in %s\n", center.c_str(), preprocessed.c_str());
|
||||
return 1;
|
||||
}
|
||||
const auto center_index = static_cast<std::size_t>(center_it - snapshots.begin());
|
||||
|
||||
const auto run_config = radar::config::load_run_config(root + "/config_profile.json");
|
||||
if (run_config.gpr.tx_geometry.empty() || run_config.gpr.rx_geometry.empty()) {
|
||||
std::printf("FAIL: config_profile.json has no gpr geometry\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
auto live = build_live_config(run_config);
|
||||
live.sar3d_volume_dump_dir = dump_dir;
|
||||
const auto radius = static_cast<std::size_t>(live.sar3d_frame_radius);
|
||||
if (center_index < radius || center_index + radius >= snapshots.size()) {
|
||||
std::printf("not enough snapshots around center for radius %zu\n", radius);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// The processor centers the aperture `radius` frames before the newest
|
||||
// collection, so feeding [center-r .. center+r] in order reproduces the
|
||||
// symmetric aperture around `center`.
|
||||
std::vector<radar::ipc::PreprocessedCollection> history{};
|
||||
for (std::size_t index = center_index - radius; index <= center_index + radius; ++index) {
|
||||
history.push_back(load_collection(run_config, root, snapshots[index], index));
|
||||
}
|
||||
std::printf("aperture: %zu frames around %s\n", history.size(), center.c_str());
|
||||
|
||||
radar::processing::Sar3dProcessor processor{};
|
||||
radar::ipc::ResultCollection result{};
|
||||
for (std::size_t index = 0U; index < history.size(); ++index) {
|
||||
result = processor.process_collection(
|
||||
run_config,
|
||||
history[index],
|
||||
std::span<const radar::ipc::PreprocessedCollection>(history.data(), index),
|
||||
live
|
||||
);
|
||||
}
|
||||
|
||||
if (result.collection_payloads.empty()) {
|
||||
std::printf("FAIL: sar3d produced no payloads\n");
|
||||
return 1;
|
||||
}
|
||||
for (const auto& payload : result.collection_payloads) {
|
||||
if (payload.kind == radar::ipc::ResultKind::ScalarF32) {
|
||||
std::printf("%s (volume level): %.9e\n",
|
||||
payload.processing_name.c_str(), static_cast<double>(payload.scalar_value));
|
||||
}
|
||||
}
|
||||
for (const auto& payload : result.collection_payloads) {
|
||||
if (payload.kind == radar::ipc::ResultKind::ImageF32 && payload.processing_name == "sar3d_proj_xz") {
|
||||
print_projection_argmax(payload, "X", "Z");
|
||||
}
|
||||
}
|
||||
|
||||
// Bottom view across the selectable angles: the target's u coordinate must
|
||||
// move smoothly from its X (front, 0 deg) to its Z (side, 90 deg).
|
||||
const auto last_previous =
|
||||
std::span<const radar::ipc::PreprocessedCollection>(history.data(), history.size() - 1U);
|
||||
for (const float angle_deg : {0.0F, 30.0F, 60.0F, 90.0F}) {
|
||||
auto live_angle = live;
|
||||
live_angle.sar3d_volume_dump_dir.clear();
|
||||
live_angle.sar3d_bottom_view_deg = angle_deg;
|
||||
const auto angle_result =
|
||||
processor.process_collection(run_config, history.back(), last_previous, live_angle);
|
||||
for (const auto& payload : angle_result.collection_payloads) {
|
||||
if (payload.kind == radar::ipc::ResultKind::ImageF32 && payload.processing_name == "sar3d_proj_bottom") {
|
||||
std::printf("bottom @ %2.0f deg -> ", static_cast<double>(angle_deg));
|
||||
print_projection_argmax(payload, "u", "Y");
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!dump_dir.empty()) {
|
||||
std::printf("volume dumped to %s/sar3d_volume_%llu.{f32,json}\n",
|
||||
dump_dir.c_str(),
|
||||
static_cast<unsigned long long>(history.back().collection_id));
|
||||
}
|
||||
std::printf("OK: sar3d processor path completed\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -1,110 +0,0 @@
|
||||
# Коллектор радиофотонного радара (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** дочитывает и закрывает собственный дочерний процесс сервиса. Завершения
|
||||
процессов по имени нет.
|
||||
@@ -1,98 +0,0 @@
|
||||
#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
|
||||
@@ -1,530 +0,0 @@
|
||||
#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;
|
||||
}
|
||||
}
|
||||
@@ -1,317 +0,0 @@
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
#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
@@ -0,0 +1,24 @@
|
||||
#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
@@ -0,0 +1,414 @@
|
||||
#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
@@ -0,0 +1,71 @@
|
||||
#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
@@ -0,0 +1,409 @@
|
||||
#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
@@ -0,0 +1,148 @@
|
||||
#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
@@ -0,0 +1,394 @@
|
||||
#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
@@ -0,0 +1,197 @@
|
||||
#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
@@ -0,0 +1,112 @@
|
||||
#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
@@ -0,0 +1,271 @@
|
||||
#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
@@ -0,0 +1,34 @@
|
||||
#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
|
||||
+4
@@ -83,6 +83,10 @@ 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);
|
||||
+7
-5
@@ -4,6 +4,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "run_config.hpp"
|
||||
#include "switch_driver.hpp"
|
||||
|
||||
namespace radar::drivers {
|
||||
|
||||
@@ -26,14 +27,15 @@ struct H7992MinimalDriverSettings {
|
||||
* Native mode uses Linux GPIO character-device API (uAPI v2).
|
||||
* Mock mode keeps state in memory without touching hardware.
|
||||
*/
|
||||
class H7992MinimalDriver {
|
||||
class H7992MinimalDriver final : public SwitchDriver {
|
||||
public:
|
||||
explicit H7992MinimalDriver(H7992MinimalDriverSettings settings);
|
||||
~H7992MinimalDriver(); // Leaves RF path safe even if close() was skipped.
|
||||
~H7992MinimalDriver() override; // Leaves RF path safe even if close() was skipped.
|
||||
|
||||
void open();
|
||||
void close();
|
||||
void switch_to(std::uint32_t position);
|
||||
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();
|
||||
+183
@@ -0,0 +1,183 @@
|
||||
#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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user