init commit
This commit is contained in:
Ayzen
@@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "processor_interface.hpp"
|
||||
#include "processing_live_config.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
using ProcessorRegistry = std::unordered_map<std::string, std::unique_ptr<ProcessorInterface>>;
|
||||
|
||||
class DataProcessor {
|
||||
public:
|
||||
DataProcessor(
|
||||
const config::RunConfig& config,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing& results_ring,
|
||||
ProcessorRegistry processors
|
||||
);
|
||||
|
||||
void run(const std::atomic<bool>& stop_requested);
|
||||
|
||||
private:
|
||||
[[nodiscard]] auto process_collection(
|
||||
const ipc::PreprocessedCollection& preprocessed,
|
||||
ProcessorInterface& processor,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection;
|
||||
|
||||
[[nodiscard]] auto resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface&;
|
||||
|
||||
const config::RunConfig& config_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing& results_ring_;
|
||||
ProcessorRegistry processors_{};
|
||||
std::string default_processor_mode_{};
|
||||
ProcessingLiveConfigLoader live_config_loader_;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto create_default_processors() -> ProcessorRegistry;
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,49 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
enum class HistoryCommand {
|
||||
None,
|
||||
RemoveLast,
|
||||
ClearAll,
|
||||
};
|
||||
|
||||
struct ProcessingLiveConfig {
|
||||
std::string processor_mode = "pass_through";
|
||||
float gain_db = 0.0F;
|
||||
float phase_deg = 0.0F;
|
||||
std::string bscan_axis = "abs";
|
||||
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;
|
||||
std::uint64_t history_command_seq = 0;
|
||||
HistoryCommand history_command = HistoryCommand::None;
|
||||
};
|
||||
|
||||
class ProcessingLiveConfigLoader {
|
||||
public:
|
||||
explicit ProcessingLiveConfigLoader(std::string path);
|
||||
|
||||
[[nodiscard]] auto current() const -> ProcessingLiveConfig;
|
||||
[[nodiscard]] auto refresh_if_needed() -> ProcessingLiveConfig;
|
||||
[[nodiscard]] auto revision() const -> std::uint64_t;
|
||||
|
||||
private:
|
||||
[[nodiscard]] auto read_from_file() const -> ProcessingLiveConfig;
|
||||
|
||||
std::string path_{};
|
||||
ProcessingLiveConfig current_{};
|
||||
std::filesystem::file_time_type last_write_time_{};
|
||||
bool has_last_write_time_ = false;
|
||||
std::uint64_t revision_ = 0;
|
||||
std::chrono::steady_clock::time_point next_check_at_{};
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,156 @@
|
||||
#include "data_processor.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "bscan_processor.hpp"
|
||||
#include "passthrough_processor.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
|
||||
constexpr const char* kDefaultProcessorMode = "pass_through";
|
||||
constexpr std::size_t kBscanReplayWindow = 50U;
|
||||
|
||||
[[nodiscard]] auto replay_history_limit(const config::RunConfig& config) -> std::size_t {
|
||||
const auto preprocessed_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.preprocessed.capacity));
|
||||
const auto results_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.results.capacity));
|
||||
return std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow});
|
||||
}
|
||||
|
||||
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)) {
|
||||
throw std::runtime_error("Results ring slot is too small for serialized collection");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DataProcessor::DataProcessor(
|
||||
const config::RunConfig& config,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing& results_ring,
|
||||
ProcessorRegistry processors
|
||||
)
|
||||
: 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) {
|
||||
if (processors_.empty()) {
|
||||
throw std::runtime_error("DataProcessor requires at least one processor");
|
||||
}
|
||||
if (const auto found = processors_.find(default_processor_mode_); found == processors_.end()) {
|
||||
default_processor_mode_ = processors_.begin()->first;
|
||||
}
|
||||
}
|
||||
|
||||
void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
std::vector<std::uint8_t> bytes{};
|
||||
std::deque<ipc::PreprocessedCollection> preprocessed_history{};
|
||||
const std::size_t history_limit = replay_history_limit(config_);
|
||||
std::uint64_t last_replayed_revision = live_config_loader_.revision();
|
||||
std::uint64_t last_applied_history_command_seq = 0;
|
||||
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
const auto live_config = live_config_loader_.refresh_if_needed();
|
||||
const auto live_revision = live_config_loader_.revision();
|
||||
auto& processor = resolve_processor(live_config);
|
||||
|
||||
if (live_revision != last_replayed_revision) {
|
||||
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;
|
||||
}
|
||||
|
||||
if (live_config.processor_mode == "bscan") {
|
||||
for (const auto& cached : preprocessed_history) {
|
||||
const auto replay_result = process_collection(cached, processor, live_config);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
}
|
||||
} else if (!preprocessed_history.empty()) {
|
||||
const auto replay_result = process_collection(preprocessed_history.back(), processor, live_config);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
}
|
||||
last_replayed_revision = live_revision;
|
||||
}
|
||||
|
||||
if (preprocessed_ring_.pop(bytes)) {
|
||||
auto preprocessed = ipc::deserialize_preprocessed_collection(bytes);
|
||||
preprocessed_history.push_back(std::move(preprocessed));
|
||||
while (preprocessed_history.size() > history_limit) {
|
||||
preprocessed_history.pop_front();
|
||||
}
|
||||
|
||||
const auto result_collection = process_collection(preprocessed_history.back(), processor, live_config);
|
||||
publish_result_collection(result_collection, results_ring_);
|
||||
continue;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.runtime.idle_sleep_ms));
|
||||
}
|
||||
}
|
||||
|
||||
auto DataProcessor::process_collection(
|
||||
const ipc::PreprocessedCollection& preprocessed,
|
||||
ProcessorInterface& processor,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = preprocessed.collection_id;
|
||||
// Keep source monotonic timestamp stable across live-config replays.
|
||||
results.monotonic_ns = preprocessed.monotonic_ns;
|
||||
results.blocks.reserve(preprocessed.traces.size());
|
||||
|
||||
for (const auto& trace : preprocessed.traces) {
|
||||
ipc::ResultBlock block{};
|
||||
block.combo = trace.combo;
|
||||
block.payloads.reserve(1U);
|
||||
block.payloads.push_back(processor.process(trace, live_config));
|
||||
|
||||
results.blocks.push_back(std::move(block));
|
||||
}
|
||||
|
||||
return results;
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface& {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
|
||||
if (auto found = processors_.find(requested_mode); found != processors_.end()) {
|
||||
return *(found->second);
|
||||
}
|
||||
if (auto fallback = processors_.find(default_processor_mode_); fallback != processors_.end()) {
|
||||
return *(fallback->second);
|
||||
}
|
||||
return *(processors_.begin()->second);
|
||||
}
|
||||
|
||||
auto create_default_processors() -> ProcessorRegistry {
|
||||
ProcessorRegistry processors{};
|
||||
{
|
||||
auto processor = std::make_unique<PassThroughProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
{
|
||||
auto processor = std::make_unique<BScanProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
return processors;
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,69 @@
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include "data_processor.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_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,
|
||||
config.rings.preprocessed.capacity,
|
||||
config.rings.preprocessed.slot_size_bytes
|
||||
);
|
||||
auto results_ring = radar::ipc::ShmRing::open_or_create(
|
||||
config.rings.results.name,
|
||||
config.rings.results.capacity,
|
||||
config.rings.results.slot_size_bytes
|
||||
);
|
||||
|
||||
radar::processing::DataProcessor processor(
|
||||
config,
|
||||
preprocessed_ring,
|
||||
results_ring,
|
||||
radar::processing::create_default_processors()
|
||||
);
|
||||
processor.run(g_stop_requested);
|
||||
return 0;
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_processor error: " << exception.what() << '\n';
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
#include "processing_live_config.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace radar::processing {
|
||||
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_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");
|
||||
}
|
||||
const double numeric = value.get<double>();
|
||||
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 (std::fabs(numeric - rounded) > 1e-6) {
|
||||
throw std::runtime_error(field_name + " must be integer");
|
||||
}
|
||||
return static_cast<std::uint64_t>(rounded);
|
||||
}
|
||||
|
||||
[[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);
|
||||
}
|
||||
|
||||
Json root{};
|
||||
try {
|
||||
root = Json::parse(json_text);
|
||||
} catch (const Json::parse_error& error) {
|
||||
throw std::runtime_error("Failed to parse processing live config: " + std::string(error.what()));
|
||||
}
|
||||
|
||||
if (!root.is_object()) {
|
||||
throw std::runtime_error("Processing live config root must be JSON object");
|
||||
}
|
||||
|
||||
ProcessingLiveConfig config{};
|
||||
if (const auto found = root.find("processor_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.processor_mode must be string");
|
||||
}
|
||||
config.processor_mode = found->get<std::string>();
|
||||
}
|
||||
if (const auto found = root.find("gain_db"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gain_db must be number");
|
||||
}
|
||||
config.gain_db = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("phase_deg"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.phase_deg must be number");
|
||||
}
|
||||
config.phase_deg = 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_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("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)
|
||||
: path_(std::move(path)),
|
||||
next_check_at_(std::chrono::steady_clock::now()) {
|
||||
if (path_.empty()) {
|
||||
throw std::runtime_error("Processing live config path must not be empty");
|
||||
}
|
||||
}
|
||||
|
||||
auto ProcessingLiveConfigLoader::current() const -> ProcessingLiveConfig {
|
||||
return current_;
|
||||
}
|
||||
|
||||
auto ProcessingLiveConfigLoader::revision() const -> std::uint64_t {
|
||||
return revision_;
|
||||
}
|
||||
|
||||
auto ProcessingLiveConfigLoader::refresh_if_needed() -> ProcessingLiveConfig {
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (now < next_check_at_) {
|
||||
return current_;
|
||||
}
|
||||
next_check_at_ = now + std::chrono::milliseconds(100);
|
||||
|
||||
std::error_code time_error{};
|
||||
const auto file_time = std::filesystem::last_write_time(path_, time_error);
|
||||
if (time_error) {
|
||||
return current_;
|
||||
}
|
||||
|
||||
if (has_last_write_time_ && file_time == last_write_time_) {
|
||||
return current_;
|
||||
}
|
||||
|
||||
try {
|
||||
current_ = read_from_file();
|
||||
last_write_time_ = file_time;
|
||||
has_last_write_time_ = true;
|
||||
++revision_;
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << "data_processor warning: failed to refresh live processing config: " << error.what() << '\n';
|
||||
}
|
||||
|
||||
return current_;
|
||||
}
|
||||
|
||||
auto ProcessingLiveConfigLoader::read_from_file() const -> ProcessingLiveConfig {
|
||||
std::ifstream stream(path_);
|
||||
if (!stream.is_open()) {
|
||||
throw std::runtime_error("Failed to open live processing config: " + path_);
|
||||
}
|
||||
|
||||
std::stringstream buffer{};
|
||||
buffer << stream.rdbuf();
|
||||
return parse_live_config(buffer.str(), path_);
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
|
||||
#include "processor_interface.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
class BScanProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
[[nodiscard]] auto name() const -> std::string override;
|
||||
[[nodiscard]] auto process(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultPayload override;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
|
||||
#include "processor_interface.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
class PassThroughProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
[[nodiscard]] auto name() const -> std::string override;
|
||||
[[nodiscard]] auto process(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultPayload override;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "processing_live_config.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
class ProcessorInterface {
|
||||
public:
|
||||
virtual ~ProcessorInterface() = default;
|
||||
|
||||
[[nodiscard]] virtual auto name() const -> std::string = 0;
|
||||
[[nodiscard]] virtual auto process(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultPayload = 0;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,237 @@
|
||||
#include "bscan_processor.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <string_view>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kSpeedOfLightMetersPerSec = 299'792'458.0;
|
||||
|
||||
struct BScanProfile {
|
||||
std::vector<float> depth_m{};
|
||||
std::vector<float> response{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto next_power_of_two(std::size_t value) -> std::size_t {
|
||||
if (value <= 1U) {
|
||||
return 1U;
|
||||
}
|
||||
|
||||
std::size_t power = 1U;
|
||||
while (power < value) {
|
||||
if (power > (std::numeric_limits<std::size_t>::max() >> 1U)) {
|
||||
return value;
|
||||
}
|
||||
power <<= 1U;
|
||||
}
|
||||
return power;
|
||||
}
|
||||
|
||||
void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
|
||||
const std::size_t size = values.size();
|
||||
if (size <= 1U) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (std::size_t index = 1U, bit_reversed = 0U; index < size; ++index) {
|
||||
std::size_t bit = size >> 1U;
|
||||
while (bit_reversed & bit) {
|
||||
bit_reversed ^= bit;
|
||||
bit >>= 1U;
|
||||
}
|
||||
bit_reversed ^= bit;
|
||||
if (index < bit_reversed) {
|
||||
std::swap(values[index], values[bit_reversed]);
|
||||
}
|
||||
}
|
||||
|
||||
for (std::size_t len = 2U; len <= size; len <<= 1U) {
|
||||
const double angle = 2.0 * kPi * (inverse ? 1.0 : -1.0) / static_cast<double>(len);
|
||||
const std::complex<double> twiddle_step(std::cos(angle), std::sin(angle));
|
||||
const std::size_t half_len = len >> 1U;
|
||||
|
||||
for (std::size_t offset = 0U; offset < size; offset += len) {
|
||||
std::complex<double> twiddle(1.0, 0.0);
|
||||
for (std::size_t i = 0U; i < half_len; ++i) {
|
||||
const auto even = values[offset + i];
|
||||
const auto odd = values[offset + i + half_len] * twiddle;
|
||||
|
||||
values[offset + i] = even + odd;
|
||||
values[offset + i + half_len] = even - odd;
|
||||
twiddle *= twiddle_step;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!inverse) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double scale = 1.0 / static_cast<double>(size);
|
||||
for (auto& value : values) {
|
||||
value *= scale;
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto select_axis(std::complex<double> sample, std::string_view axis) -> double {
|
||||
if (axis == "real") {
|
||||
return std::real(sample);
|
||||
}
|
||||
if (axis == "phase") {
|
||||
return std::arg(sample);
|
||||
}
|
||||
return std::abs(sample);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto fallback_profile(const ipc::SweepTraceBlock& trace) -> BScanProfile {
|
||||
const std::size_t point_count = std::min(trace.frequency_hz.size(), trace.s21.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 = trace.s21[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 std::size_t point_count = std::min(trace.frequency_hz.size(), trace.s21.size());
|
||||
if (point_count < 2U) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
const double configured_start_hz = static_cast<double>(live_config.bscan_start_freq_mhz) * 1'000'000.0;
|
||||
const double configured_stop_hz = static_cast<double>(live_config.bscan_stop_freq_mhz) * 1'000'000.0;
|
||||
const double start_hz = std::min(configured_start_hz, configured_stop_hz);
|
||||
const double stop_hz = std::max(configured_start_hz, configured_stop_hz);
|
||||
|
||||
std::vector<double> filtered_freq_hz{};
|
||||
std::vector<std::complex<double>> filtered_s21{};
|
||||
filtered_freq_hz.reserve(point_count);
|
||||
filtered_s21.reserve(point_count);
|
||||
|
||||
for (std::size_t index = 0U; index < point_count; ++index) {
|
||||
const double frequency_hz = static_cast<double>(trace.frequency_hz[index]);
|
||||
if (frequency_hz < start_hz || frequency_hz > stop_hz) {
|
||||
continue;
|
||||
}
|
||||
const auto& sample = trace.s21[index];
|
||||
filtered_freq_hz.push_back(frequency_hz);
|
||||
filtered_s21.emplace_back(static_cast<double>(sample.re), static_cast<double>(sample.im));
|
||||
}
|
||||
|
||||
if (filtered_freq_hz.size() < 2U) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
const std::size_t filtered_count = filtered_freq_hz.size();
|
||||
const double df = (filtered_freq_hz.back() - filtered_freq_hz.front()) / static_cast<double>(filtered_count - 1U);
|
||||
if (df <= 0.0) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(filtered_freq_hz.front() / df));
|
||||
if (start_bin < 0) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
const auto start_index = static_cast<std::size_t>(start_bin);
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() / 2U)) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
if (start_index > (std::numeric_limits<std::size_t>::max() - filtered_count + 1U)) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
const std::size_t min_fft_len = 2U * (start_index + filtered_count - 1U);
|
||||
const std::size_t fft_len = next_power_of_two(min_fft_len);
|
||||
if (fft_len < min_fft_len || (fft_len & (fft_len - 1U)) != 0U) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
if (start_index > fft_len || filtered_count > (fft_len - start_index)) {
|
||||
return fallback_profile(trace);
|
||||
}
|
||||
|
||||
std::vector<std::complex<double>> spectrum(fft_len, std::complex<double>(0.0, 0.0));
|
||||
for (std::size_t index = 0U; index < filtered_count; ++index) {
|
||||
spectrum[start_index + index] = filtered_s21[index];
|
||||
}
|
||||
|
||||
fft_inplace(spectrum, true);
|
||||
|
||||
const double dt = 1.0 / (static_cast<double>(fft_len) * df);
|
||||
const double cut_m = std::max(0.0, static_cast<double>(live_config.bscan_cut_m));
|
||||
const double max_depth_m = std::max(0.0, static_cast<double>(live_config.bscan_max_depth_m));
|
||||
const double gain = static_cast<double>(live_config.bscan_gain);
|
||||
const double window_start = 2.0 * cut_m;
|
||||
const double window_stop = window_start + (2.0 * max_depth_m);
|
||||
|
||||
BScanProfile profile{};
|
||||
profile.depth_m.reserve(spectrum.size());
|
||||
profile.response.reserve(spectrum.size());
|
||||
|
||||
const std::string_view axis = live_config.bscan_axis;
|
||||
for (std::size_t index = 0U; index < spectrum.size(); ++index) {
|
||||
const double depth_raw = static_cast<double>(index) * dt * kSpeedOfLightMetersPerSec;
|
||||
if (depth_raw < window_start || depth_raw > window_stop) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const double one_way_depth = (depth_raw - window_start) / 2.0;
|
||||
double gain_shape = std::pow(one_way_depth, gain);
|
||||
if (!std::isfinite(gain_shape)) {
|
||||
gain_shape = 0.0;
|
||||
}
|
||||
|
||||
const double axis_value = select_axis(spectrum[index], axis);
|
||||
const double output_value = axis_value * gain_shape;
|
||||
|
||||
profile.depth_m.push_back(static_cast<float>(one_way_depth));
|
||||
profile.response.push_back(static_cast<float>(output_value));
|
||||
}
|
||||
|
||||
return profile;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
auto BScanProcessor::name() const -> std::string {
|
||||
return "bscan";
|
||||
}
|
||||
|
||||
auto BScanProcessor::process(const ipc::SweepTraceBlock& trace, const ProcessingLiveConfig& live_config)
|
||||
-> ipc::ResultPayload {
|
||||
ipc::ResultPayload payload{};
|
||||
payload.processing_name = name();
|
||||
payload.kind = ipc::ResultKind::TraceComplex;
|
||||
|
||||
auto profile = compute_bscan_profile(trace, live_config);
|
||||
payload.frequency_hz = std::move(profile.depth_m);
|
||||
payload.trace.reserve(profile.response.size());
|
||||
for (const auto value : profile.response) {
|
||||
payload.trace.push_back(ipc::Complex32{
|
||||
.re = value,
|
||||
.im = 0.0F,
|
||||
});
|
||||
}
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,39 @@
|
||||
#include "passthrough_processor.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
|
||||
constexpr float kPi = 3.14159265358979323846F;
|
||||
|
||||
} // namespace
|
||||
|
||||
auto PassThroughProcessor::name() const -> std::string {
|
||||
return "pass_through";
|
||||
}
|
||||
|
||||
auto PassThroughProcessor::process(const ipc::SweepTraceBlock& trace, const ProcessingLiveConfig& live_config)
|
||||
-> ipc::ResultPayload {
|
||||
ipc::ResultPayload payload{};
|
||||
payload.processing_name = name();
|
||||
payload.kind = ipc::ResultKind::TraceComplex;
|
||||
payload.frequency_hz = trace.frequency_hz;
|
||||
payload.trace = trace.s21;
|
||||
|
||||
const float linear_gain = std::pow(10.0F, live_config.gain_db / 20.0F);
|
||||
const float phase_rad = live_config.phase_deg * (kPi / 180.0F);
|
||||
const float cos_phase = std::cos(phase_rad);
|
||||
const float sin_phase = std::sin(phase_rad);
|
||||
|
||||
for (auto& sample : payload.trace) {
|
||||
const float re = sample.re;
|
||||
const float im = sample.im;
|
||||
sample.re = linear_gain * ((re * cos_phase) - (im * sin_phase));
|
||||
sample.im = linear_gain * ((re * sin_phase) + (im * cos_phase));
|
||||
}
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
Reference in New Issue
Block a user