awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rework radar system

Browse Source
This commit is contained in:
Ayzen
2026-10-06 12:32:26 +03:00
parent 552cdc688a
commit 5bb86b7165
299 changed files with 13537 additions and 27763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/common_cpp/ipc/src/shared_types.cpp
+13 -169
View File
@@ -1,5 +1,6 @@
#include "shared_types.hpp"
#include <bit>
#include <chrono>
#include <cstring>
#include <limits>
@@ -12,9 +13,11 @@
namespace radar::ipc {
namespace {
constexpr std::uint32_t kRawCollectionMagic = 0x32574152U; // RAW2
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x32525050U; // PRP2
constexpr std::uint32_t kResultCollectionMagic = 0x324C5352U; // RSL2
static_assert(std::endian::native == std::endian::little, "IPC requires a little-endian host");
constexpr std::uint32_t kRawCollectionMagic = 0x33574152U; // RAW3
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x33505250U; // PRP3
constexpr std::uint32_t kResultCollectionMagic = 0x334C5352U; // RSL3
template <typename T>
concept TriviallySerializable = std::is_trivially_copyable_v<T>;
@@ -75,16 +78,6 @@ class BinaryReader {
return value;
}
[[nodiscard]] auto read_string() -> std::string {
const auto size = read<std::uint16_t>();
ensure_available(size);
const auto* begin = reinterpret_cast<const char*>(bytes_.data() + offset_);
std::string value(begin, begin + size);
offset_ += size;
return value;
}
[[nodiscard]] auto is_consumed() const -> bool {
return offset_ == bytes_.size();
}
@@ -106,7 +99,6 @@ class BinaryReader {
void write_trace_block(BinaryWriter& writer, const SweepTraceBlock& trace) {
ensure_equal_sizes(trace.frequency_hz.size(), trace.s21.size(), "Sweep trace");
ensure_equal_sizes(trace.frequency_hz.size(), trace.s11.size(), "Sweep trace S11");
writer.write(trace.combo.input_pos);
writer.write(trace.combo.output_pos);
@@ -116,11 +108,6 @@ void write_trace_block(BinaryWriter& writer, const SweepTraceBlock& trace) {
writer.write(frequency_hz);
}
for (const auto& point : trace.s11) {
writer.write(point.re);
writer.write(point.im);
}
for (const auto& point : trace.s21) {
writer.write(point.re);
writer.write(point.im);
@@ -144,23 +131,15 @@ 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) + s11 (8B) + s21 (8B) = 20 bytes.
require_count_fits(point_count, 20U, reader);
// Each point encodes frequency (4B) + s21 (8B) = 12 bytes.
require_count_fits(point_count, 12U, reader);
trace.frequency_hz.reserve(point_count);
trace.s11.reserve(point_count);
trace.s21.reserve(point_count);
for (std::uint32_t index = 0; index < point_count; ++index) {
trace.frequency_hz.push_back(reader.read<float>());
}
for (std::uint32_t index = 0; index < point_count; ++index) {
trace.s11.push_back(Complex32{
.re = reader.read<float>(),
.im = reader.read<float>(),
});
}
for (std::uint32_t index = 0; index < point_count; ++index) {
trace.s21.push_back(Complex32{
.re = reader.read<float>(),
@@ -171,10 +150,7 @@ void require_count_fits(std::uint64_t count, std::size_t min_bytes_each, BinaryR
return trace;
}
// The capture windows live in a TRAILER after the trace blocks rather than inside
// them, so a reader built before they existed still decodes every trace and simply
// stops early. The trailer grows the same way: collection window first, then the
// per-trace window table (one pair per trace, in trace order).
// Capture trailer: collection window, then one per-trace window in trace order.
void write_trace_collection(BinaryWriter& writer, std::uint32_t magic, const RawSweepCollection& collection) {
writer.write(magic);
writer.write(collection.collection_id);
@@ -213,22 +189,9 @@ void write_trace_collection(BinaryWriter& writer, std::uint32_t magic, const Raw
collection.traces.push_back(read_trace_block(reader));
}
// Each trailer stage is optional: a payload from an older producer stops after
// the trace blocks (or after the collection window) and leaves the rest zeroed.
if (reader.remaining_bytes() == 0U) {
return collection;
}
if (reader.remaining_bytes() < (sizeof(std::uint64_t) * 2U)) {
throw std::runtime_error("Truncated capture window in trace collection");
}
collection.capture_start_ns = reader.read<std::uint64_t>();
collection.capture_end_ns = reader.read<std::uint64_t>();
if (reader.remaining_bytes() == 0U) {
return collection;
}
const auto trace_time_count = reader.read<std::uint32_t>();
if (trace_time_count != collection.traces.size()) {
throw std::runtime_error("Per-trace capture window count does not match trace count");
@@ -254,24 +217,6 @@ void write_trace_result_payload(BinaryWriter& writer, const ResultPayload& paylo
}
}
auto read_trace_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
const auto point_count = reader.read<std::uint32_t>();
require_count_fits(point_count, 12U, reader);
payload->frequency_hz.reserve(point_count);
payload->trace.reserve(point_count);
for (std::uint32_t index = 0; index < point_count; ++index) {
payload->frequency_hz.push_back(reader.read<float>());
}
for (std::uint32_t index = 0; index < point_count; ++index) {
payload->trace.push_back(Complex32{
.re = reader.read<float>(),
.im = reader.read<float>(),
});
}
}
void write_image_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
const auto x_count = checked_count_to_u32(payload.image_x_axis.size(), "Result image x axis point count");
const auto y_count = checked_count_to_u32(payload.image_y_axis.size(), "Result image y axis point count");
@@ -293,27 +238,6 @@ void write_image_result_payload(BinaryWriter& writer, const ResultPayload& paylo
}
}
auto read_image_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
const auto x_count = reader.read<std::uint32_t>();
const auto y_count = reader.read<std::uint32_t>();
const auto value_count = static_cast<std::uint64_t>(x_count) * y_count;
require_count_fits(value_count + x_count + y_count, sizeof(float), reader);
payload->image_x_axis.reserve(x_count);
payload->image_y_axis.reserve(y_count);
payload->image_values.reserve(static_cast<std::size_t>(value_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 < value_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()) {
@@ -333,21 +257,6 @@ void write_table_result_payload(BinaryWriter& writer, const ResultPayload& paylo
}
}
auto read_table_result_payload(BinaryReader& reader, ResultPayload* payload) -> void {
const auto column_count = reader.read<std::uint32_t>();
const auto row_count = reader.read<std::uint32_t>();
if (column_count == 0U && row_count != 0U) {
throw std::runtime_error("Result table cannot have rows without columns");
}
const auto value_count = static_cast<std::uint64_t>(column_count) * row_count;
require_count_fits(value_count, sizeof(float), reader);
payload->table_columns = column_count;
payload->table_values.reserve(static_cast<std::size_t>(value_count));
for (std::size_t index = 0; index < value_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);
@@ -370,29 +279,6 @@ void write_result_payload(BinaryWriter& writer, const ResultPayload& payload) {
}
}
[[nodiscard]] auto read_result_payload(BinaryReader& reader) -> ResultPayload {
ResultPayload payload{};
payload.kind = static_cast<ResultKind>(reader.read<std::uint8_t>());
payload.processing_name = reader.read_string();
switch (payload.kind) {
case ResultKind::TraceComplex:
read_trace_result_payload(reader, &payload);
return payload;
case ResultKind::ScalarF32:
payload.scalar_value = reader.read<float>();
return payload;
case ResultKind::ImageF32:
read_image_result_payload(reader, &payload);
return payload;
case ResultKind::TableF32:
read_table_result_payload(reader, &payload);
return payload;
default:
throw std::runtime_error("Unsupported result payload kind in stream");
}
}
void write_result_block(BinaryWriter& writer, const ResultBlock& block) {
writer.write(block.combo.input_pos);
writer.write(block.combo.output_pos);
@@ -403,21 +289,6 @@ void write_result_block(BinaryWriter& writer, const ResultBlock& block) {
}
}
[[nodiscard]] auto read_result_block(BinaryReader& reader) -> ResultBlock {
ResultBlock block{};
block.combo.input_pos = reader.read<std::uint32_t>();
block.combo.output_pos = reader.read<std::uint32_t>();
const auto payload_count = reader.read<std::uint32_t>();
require_count_fits(payload_count, 3U, reader);
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);
@@ -458,6 +329,10 @@ auto serialize_result_collection(const ResultCollection& collection) -> std::vec
writer.write(collection.collection_id);
writer.write(collection.monotonic_ns);
writer.write(collection.processing_duration_ns);
writer.write(collection.settings_revision);
writer.write(collection.capture_start_ns);
writer.write(collection.capture_end_ns);
writer.write(collection.speed_m_s);
writer.write(checked_count_to_u32(collection.collection_payloads.size(), "Collection payload count"));
writer.write(checked_count_to_u32(collection.blocks.size(), "Result block count"));
@@ -471,37 +346,6 @@ auto serialize_result_collection(const ResultCollection& collection) -> std::vec
return std::move(writer).finish();
}
auto deserialize_result_collection(std::span<const std::uint8_t> bytes) -> ResultCollection {
BinaryReader reader(bytes);
const auto magic = reader.read<std::uint32_t>();
if (magic != kResultCollectionMagic) {
throw std::runtime_error("Unexpected result collection magic");
}
ResultCollection collection{};
collection.collection_id = reader.read<std::uint64_t>();
collection.monotonic_ns = reader.read<std::uint64_t>();
collection.processing_duration_ns = reader.read<std::uint64_t>();
const auto collection_payload_count = reader.read<std::uint32_t>();
const auto block_count = reader.read<std::uint32_t>();
require_count_fits(collection_payload_count, 3U, reader);
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));
}
require_count_fits(block_count, 12U, 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();
data_acq_and_processing/common_cpp/ipc/src/shm_ring.cpp
+28 -175
View File
@@ -44,63 +44,10 @@ 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() {
@@ -129,28 +76,7 @@ auto ShmRing::open_or_create(
std::uint32_t slot_size_bytes
) -> ShmRing {
validate_name(name);
if (capacity == 0U) {
throw std::runtime_error("Ring capacity must be > 0");
}
if (slot_size_bytes == 0U) {
throw std::runtime_error("Ring slot size must be > 0");
}
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);
}
const auto mapped_size = static_cast<std::size_t>(mapped_size_u64);
const auto mapped_size = checked_mapped_size(capacity, slot_size_bytes, name);
bool created = false;
int fd = shm_open(name.c_str(), O_RDWR | O_CREAT | O_EXCL, 0660);
@@ -164,8 +90,8 @@ auto ShmRing::open_or_create(
throw errno_message("Failed to open shared memory ring", name);
}
ScopedFd scoped_fd(fd);
ShmRing ring{};
ring.fd_ = fd;
if (created) {
if (ftruncate(fd, static_cast<off_t>(mapped_size)) != 0) {
@@ -195,30 +121,29 @@ auto ShmRing::open_or_create(
throw errno_message("Failed to mmap shared memory ring", name);
}
ScopedMmap scoped_mmap(mapped, mapped_size);
auto* header = static_cast<Header*>(mapped);
ring.mapped_ = mapped;
ring.mapped_size_ = mapped_size;
auto* header = ring.header_ = static_cast<Header*>(mapped);
if (created) {
// ftruncate already supplies zero-filled slots; leave them demand-paged
// instead of touching the full ring before any payload is published.
std::memset(mapped, 0, sizeof(Header));
std::memcpy(header->magic, kRingMagic.data(), kRingMagic.size());
header->version = kRingVersion;
header->capacity = capacity;
header->slot_size_bytes = slot_size_bytes;
new (&header->write_seq) std::atomic<std::uint64_t>(0);
new (&header->read_seq) std::atomic<std::uint64_t>(0);
new (&header->dropped) std::atomic<std::uint64_t>(0);
std::atomic_thread_fence(std::memory_order_release);
// Publish readiness only after geometry and cursors have been initialized.
std::atomic_ref(header->version).store(kRingVersion, std::memory_order_release);
} else {
const auto deadline = std::chrono::steady_clock::now() + kHeaderInitWaitTimeout;
while (true) {
std::atomic_thread_fence(std::memory_order_acquire);
const bool version_ok = std::atomic_ref(header->version).load(std::memory_order_acquire) == kRingVersion;
const bool magic_ok = std::memcmp(header->magic, kRingMagic.data(), kRingMagic.size()) == 0;
const bool version_ok = header->version == kRingVersion;
const bool geometry_ok = header->capacity == capacity && header->slot_size_bytes == slot_size_bytes;
if (magic_ok && version_ok && geometry_ok) {
@@ -247,89 +172,13 @@ auto ShmRing::open_or_create(
}
}
ShmRing ring{};
ring.fd_ = scoped_fd.release();
const auto [released_mapped, released_size] = scoped_mmap.release();
ring.mapped_ = released_mapped;
ring.mapped_size_ = released_size;
ring.header_ = static_cast<Header*>(released_mapped);
return ring;
}
auto ShmRing::open_existing(const std::string& name) -> ShmRing {
validate_name(name);
const int fd = shm_open(name.c_str(), O_RDWR, 0660);
if (fd < 0) {
throw errno_message("Failed to open shared memory ring", name);
}
ScopedFd scoped_fd(fd);
struct stat info {};
if (fstat(fd, &info) != 0) {
throw errno_message("Failed to stat shared memory ring", name);
}
if (info.st_size < static_cast<off_t>(sizeof(Header))) {
throw std::runtime_error("Shared memory ring size is too small for " + name);
}
const auto mapped_size = static_cast<std::size_t>(info.st_size);
void* mapped = mmap(nullptr, mapped_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
if (mapped == MAP_FAILED) {
throw errno_message("Failed to mmap shared memory ring", name);
}
ScopedMmap scoped_mmap(mapped, mapped_size);
auto* header = static_cast<Header*>(mapped);
if (std::memcmp(header->magic, kRingMagic.data(), kRingMagic.size()) != 0) {
throw std::runtime_error("Shared memory ring magic mismatch for " + name);
}
if (header->version != kRingVersion) {
throw std::runtime_error("Shared memory ring version mismatch for " + name);
}
// ensure the mapping actually spans every slot the header describes.
validate_geometry(*header, mapped_size, name);
ShmRing ring{};
ring.fd_ = scoped_fd.release();
const auto [released_mapped, released_size] = scoped_mmap.release();
ring.mapped_ = released_mapped;
ring.mapped_size_ = released_size;
ring.header_ = static_cast<Header*>(released_mapped);
return ring;
}
void ShmRing::unlink_ring(const std::string& name) {
validate_name(name);
if (shm_unlink(name.c_str()) != 0 && errno != ENOENT) {
throw errno_message("Failed to unlink shared memory ring", name);
}
}
auto ShmRing::is_open() const -> bool {
return header_ != nullptr;
}
auto ShmRing::capacity() const -> std::uint32_t {
return header_ != nullptr ? header_->capacity : 0;
}
auto ShmRing::slot_size_bytes() const -> std::uint32_t {
return header_ != nullptr ? header_->slot_size_bytes : 0;
}
auto ShmRing::dropped_count() const -> std::uint64_t {
if (header_ == nullptr) {
return 0;
}
return header_->dropped.load(std::memory_order_acquire);
}
auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
if (header_ == nullptr) {
throw std::runtime_error("Ring is not open");
@@ -341,7 +190,7 @@ auto ShmRing::push(std::span<const std::uint8_t> payload) -> bool {
const auto write_seq = header_->write_seq.load(std::memory_order_relaxed);
auto read_seq = header_->read_seq.load(std::memory_order_acquire);
if (checked_u64_diff(write_seq, read_seq) >= header_->capacity) {
if (write_seq >= read_seq && write_seq - read_seq >= header_->capacity) {
// Ring full: overwrite the oldest unread slot. Advance read_seq via CAS so a
// concurrent consumer's advance is never clobbered (read_seq never moves back).
header_->read_seq.compare_exchange_strong(
@@ -444,37 +293,41 @@ void ShmRing::close() {
}
void ShmRing::validate_name(const std::string& name) {
if (name.empty() || name.front() != '/') {
throw std::runtime_error("POSIX shm name must start with '/'");
if (name.size() < 2U || name.front() != '/' || name.find('/', 1) != std::string::npos
|| name.find('\0') != std::string::npos) {
throw std::runtime_error("POSIX shm name must be '/' followed by a single nonempty filename");
}
}
void ShmRing::validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name) {
// derive the expected size from the header's own geometry fields and
// require the real mapping to cover it. A bogus capacity/slot_size or a truncated
// mapping would otherwise yield out-of-bounds slot offsets and a SIGSEGV.
const std::uint32_t capacity = header.capacity;
const std::uint32_t slot_size_bytes = header.slot_size_bytes;
auto ShmRing::checked_mapped_size(
std::uint32_t capacity, std::uint32_t slot_size_bytes, const std::string& name
) -> std::size_t {
static_assert(sizeof(Header) == 64 && sizeof(SlotHeader) == 16);
if (capacity == 0U) {
throw std::runtime_error("Shared memory ring capacity is zero for " + name);
throw std::runtime_error("Ring capacity must be > 0");
}
if (slot_size_bytes == 0U) {
throw std::runtime_error("Shared memory ring slot size is zero for " + name);
throw std::runtime_error("Ring slot size must be > 0");
}
if (slot_size_bytes % alignof(SlotHeader) != 0U) {
throw std::runtime_error("Shared memory ring slot size must be a multiple of 8 for " + name);
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 expected_size = sizeof(Header) + stride * capacity;
if (expected_size > kMaxMappedSize || expected_size > std::numeric_limits<std::size_t>::max()) {
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);
}
if (mapped_size < expected_size) {
void ShmRing::validate_geometry(const Header& header, std::size_t mapped_size, const std::string& name) {
if (mapped_size < checked_mapped_size(header.capacity, header.slot_size_bytes, name)) {
throw std::runtime_error("Shared memory ring size is too small for " + name);
}
}