From 195886244a55b49f4406a226a175e28e512b4dd6 Mon Sep 17 00:00:00 2001 From: Victor Castillo Date: Sun, 8 Jun 2025 23:18:16 +0200 Subject: [PATCH] feat: added `SensorDataAggregator` & moved everything to `algorithms/libs` --- CMakeLists.txt | 4 +- src/algorithms/libs/CMakeLists.txt | 17 +++ .../sensor_data/sensor_data_aggregator.cc | 137 ++++++++++++++++++ .../libs/sensor_data/sensor_data_aggregator.h | 61 ++++++++ .../sensor_data}/sensor_data_file.cc | 0 .../sensor_data}/sensor_data_file.h | 0 .../sensor_data_source_configuration.cc | 38 ++++- .../sensor_data_source_configuration.h | 4 +- .../sensor_data}/sensor_data_type.cc | 22 ++- .../sensor_data}/sensor_data_type.h | 1 + .../sensor_data}/sensor_identifier.cc | 85 ++++++++++- .../sensor_data}/sensor_identifier.h | 13 +- .../adapters/file_source_base.cc | 2 +- .../gnuradio_blocks/CMakeLists.txt | 7 +- .../gnuradio_blocks/sensor_data_source.cc | 15 +- .../gnuradio_blocks/sensor_data_source.h | 55 +------ .../signal_source/libs/CMakeLists.txt | 8 - 17 files changed, 382 insertions(+), 87 deletions(-) create mode 100644 src/algorithms/libs/sensor_data/sensor_data_aggregator.cc create mode 100644 src/algorithms/libs/sensor_data/sensor_data_aggregator.h rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_file.cc (100%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_file.h (100%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_source_configuration.cc (87%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_source_configuration.h (95%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_type.cc (76%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_data_type.h (98%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_identifier.cc (52%) rename src/algorithms/{signal_source/libs => libs/sensor_data}/sensor_identifier.h (71%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fa82599b..848d89927 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ option(ENABLE_DMA_PROXY "Enable the use of the DMA direct to FPGA hardware, requ option(ENABLE_RAW_UDP "Enable the use of high-optimized custom UDP packet sample source, requires libpcap" OFF) -option(ENABLE_FLEXIBAND "Enable the use of the signal source adater for the Teleorbit Flexiband GNU Radio driver" OFF) +option(ENABLE_FLEXIBAND "Enable the use of the signal source adapter for the Teleorbit Flexiband GNU Radio driver" OFF) option(ENABLE_ARRAY "Enable the use of CTTC's antenna array front-end as signal source (experimental)" OFF) @@ -58,6 +58,8 @@ option(ENABLE_ZMQ "Enable GNU Radio ZeroMQ Messaging, requires gr-zeromq" ON) option(ENABLE_ION "Enable ION GNSS-SDR Metadata Standard signal source" OFF) +option(ENABLE_SENSOR_DATA "Enable sensor data source" ON) + # Performance analysis tools option(ENABLE_GPERFTOOLS "Enable linking to Gperftools libraries (tcmalloc and profiler)" OFF) diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index 3f0b050a6..9053764d8 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -67,6 +67,23 @@ set(GNSS_SPLIBS_HEADERS gnss_time.h ) +if (ENABLE_SENSOR_DATA) + set(GNSS_SPLIBS_HEADERS ${GNSS_SPLIBS_HEADERS} + sensor_data/sensor_data_type.h + sensor_data/sensor_identifier.h + sensor_data/sensor_data_file.h + sensor_data/sensor_data_source_configuration.h + sensor_data/sensor_data_aggregator.h + ) + set(GNSS_SPLIBS_SOURCES ${GNSS_SPLIBS_SOURCES} + sensor_data/sensor_data_type.cc + sensor_data/sensor_identifier.cc + sensor_data/sensor_data_file.cc + sensor_data/sensor_data_source_configuration.cc + sensor_data/sensor_data_aggregator.cc + ) +endif () + if(ENABLE_OPENCL) set(GNSS_SPLIBS_SOURCES ${GNSS_SPLIBS_SOURCES} opencl/fft_execute.cc # Needs OpenCL diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc new file mode 100644 index 000000000..e9d69e153 --- /dev/null +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc @@ -0,0 +1,137 @@ +/*! + * \file sensor_data_file.cc + * \brief Provides a simple abstraction for reading contiguous binary data from a file + * \author Victor Castillo, 2024. victorcastilloaguero(at).gmail.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "sensor_data_aggregator.h" + + +SensorDataAggregator::SensorDataAggregator(const SensorDataSourceConfiguration& configuration, const std::vector& required_sensors) +{ + std::vector missing_sensors{}; + for (const auto& required_sensor : required_sensors) + { + if (not configuration.is_sensor_provided(required_sensor)) + { + // Sensor was not provided in the configuration file + missing_sensors.push_back(required_sensor); + } + else + { + // Populate sensor sample maps + switch (SensorIdentifier::get_internal_type(required_sensor)) + { + case SensorDataType::FLOAT: + f32_data_[required_sensor] = {}; + break; + default: + break; + } + } + } + + if (not missing_sensors.empty()) + { + // TODO - Throw error if not all ok + std::stringstream ss; + ss << "ERROR: Required sensors were not provided: "; + ss << SensorIdentifier::to_string(missing_sensors[0]); + for (std::size_t i = 1; i < missing_sensors.size(); ++i) + { + ss << ", "; + ss << SensorIdentifier::to_string(missing_sensors[i]); + } + throw std::runtime_error(ss.str()); + } +} + +void SensorDataAggregator::update(const std::vector& tags) +{ + // Delete all data except last sample for each sensor + for (auto& sensor_data : f32_data_) + { + std::vector>& sensor_samples = sensor_data.second; + if (not sensor_samples.empty()) + { + SensorDataSample last_sample = sensor_samples.back(); + sensor_samples.clear(); + sensor_samples.emplace_back(last_sample); + } + } + + // Append new data + for (const auto& sensor_tag : tags) + { + if (sensor_tag.value->is_dict()) + { + append_data(sensor_tag.value); + } + } +} + +const std::vector>& SensorDataAggregator::get_f32(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + return f32_data_.at(sensor_id); +} + +SensorDataSample SensorDataAggregator::get_last_f32(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + const std::vector> samples = f32_data_.at(sensor_id); + if (samples.empty()) + { + return {0, 0}; + } + return samples.back(); +} + + +void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) +{ + static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP)); + pmt::pmt_t data_list = pmt::dict_items(data_dict); + + uint64_t sample_stamp = pmt::to_uint64(pmt::dict_ref(data_dict, SAMPLE_STAMP_KEY, pmt::from_uint64(0))); + while (not pmt::is_null(data_list)) + { + pmt::pmt_t pair = pmt::car(data_list); + pmt::pmt_t key = pmt::car(pair); + pmt::pmt_t val = pmt::cdr(pair); + + std::string key_str = pmt::write_string(key); + SensorIdentifier::value_type sensor_id = SensorIdentifier::from_string(key_str); + + if (sensor_id != SensorIdentifier::SAMPLE_STAMP and sensor_id != SensorIdentifier::CHUNK_COUNT) + { + switch (SensorIdentifier::get_internal_type(sensor_id)) + { + case SensorDataType::FLOAT: + if (f32_data_.contains(sensor_id)) + { + f32_data_.at(sensor_id).emplace_back( + sample_stamp, + pmt::to_float(val)); + } + break; + default: + break; + } + } + + data_list = pmt::cdr(data_list); + } +} diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h new file mode 100644 index 000000000..edd577fea --- /dev/null +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h @@ -0,0 +1,61 @@ +/*! + * \file sensor_data_aggregator.h + * \brief GNURadio block that adds extra data to the sample stream. + * \author Victor Castillo, 2024. victorcastilloaguero(at).gmail.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_SENSOR_DATA_AGGREGATOR_H +#define GNSS_SDR_SENSOR_DATA_AGGREGATOR_H + +#include "sensor_data_source_configuration.h" +#include "sensor_identifier.h" +#include +#include +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_libs + * \{ */ + +template +struct SensorDataSample +{ + uint64_t rf_sample_stamp{}; + DataType value{}; +}; + +class SensorDataAggregator +{ +public: + const pmt::pmt_t SENSOR_DATA_TAG = pmt::mp("sensor_data"); + + explicit SensorDataAggregator(const SensorDataSourceConfiguration& configuration, const std::vector& required_sensors); + + void update(const std::vector& tags); + + const std::vector>& get_f32(SensorIdentifier::value_type sensor_id) const; + + SensorDataSample get_last_f32(SensorIdentifier::value_type sensor_id) const; + +private: + void append_data(const pmt::pmt_t& data_dict); + + std::unordered_map>> f32_data_{}; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_SENSOR_DATA_AGGREGATOR_H diff --git a/src/algorithms/signal_source/libs/sensor_data_file.cc b/src/algorithms/libs/sensor_data/sensor_data_file.cc similarity index 100% rename from src/algorithms/signal_source/libs/sensor_data_file.cc rename to src/algorithms/libs/sensor_data/sensor_data_file.cc diff --git a/src/algorithms/signal_source/libs/sensor_data_file.h b/src/algorithms/libs/sensor_data/sensor_data_file.h similarity index 100% rename from src/algorithms/signal_source/libs/sensor_data_file.h rename to src/algorithms/libs/sensor_data/sensor_data_file.h diff --git a/src/algorithms/signal_source/libs/sensor_data_source_configuration.cc b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc similarity index 87% rename from src/algorithms/signal_source/libs/sensor_data_source_configuration.cc rename to src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc index dd7eab874..dcd563e8b 100644 --- a/src/algorithms/signal_source/libs/sensor_data_source_configuration.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.cc @@ -16,8 +16,8 @@ #include "sensor_data_source_configuration.h" -SensorDataSourceConfiguration::SensorDataSourceConfiguration(const ConfigurationInterface* configuration, bool enabled) - : enabled_(enabled), items_per_sample_(1) +SensorDataSourceConfiguration::SensorDataSourceConfiguration(const ConfigurationInterface* configuration) + : enabled_(configuration->property("SensorData.enabled"s, false)), items_per_sample_(1) { if (enabled_) { @@ -36,6 +36,25 @@ bool SensorDataSourceConfiguration::is_enabled() const return enabled_; } +bool SensorDataSourceConfiguration::is_sensor_provided(SensorIdentifier::value_type sensor_id) const +{ + if (not enabled_) + { + return false; + } + + bool sensor_found = false; + for (const auto& sensor : sensors_) + { + if (sensor_id == sensor.identifier) + { + sensor_found = true; + } + } + return sensor_found; +} + + void SensorDataSourceConfiguration::set_items_per_sample(uint64_t items_per_sample) { items_per_sample_ = items_per_sample; @@ -78,11 +97,19 @@ void SensorDataSourceConfiguration::configure_files(const ConfigurationInterface void SensorDataSourceConfiguration::configure_sensors(const ConfigurationInterface* configuration) { - uint64_t sensor_count = configuration->property(CONFIGURATION_ROLE + ".sensor_count"s, 1UL); - for (uint64_t id = 0; id < sensor_count; ++id) + for (uint64_t id = 0;; ++id) { std::string role = CONFIGURATION_ROLE + ".sensor" + std::to_string(id); + // Find out which sensor this is + const std::string sensor_identifier = configuration->property(role + ".data"s, std::string{"UNDEFINED"}); + + if (sensor_identifier == "UNDEFINED") + { + // No more sensors + break; + } + // Configure sensor data type, default to same data type as previous sensor SensorDataType::value_type data_type = SensorDataType::FLOAT; if (id > 0 and not configuration->is_present(role + ".type")) @@ -116,9 +143,6 @@ void SensorDataSourceConfiguration::configure_sensors(const ConfigurationInterfa file_id = configuration->property(role + ".file"s, 0UL); } - // Find out which sensor this is - const std::string sensor_identifier = configuration->property(role + ".data"s, std::string{"UNDEFINED"}); - if (sensor_identifier != "UNDEFINED") { sensors_.emplace_back(SensorDataConfiguration{ diff --git a/src/algorithms/signal_source/libs/sensor_data_source_configuration.h b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.h similarity index 95% rename from src/algorithms/signal_source/libs/sensor_data_source_configuration.h rename to src/algorithms/libs/sensor_data/sensor_data_source_configuration.h index ab271e25a..822089d11 100644 --- a/src/algorithms/signal_source/libs/sensor_data_source_configuration.h +++ b/src/algorithms/libs/sensor_data/sensor_data_source_configuration.h @@ -60,12 +60,14 @@ struct SensorDataConfiguration class SensorDataSourceConfiguration { public: - explicit SensorDataSourceConfiguration(const ConfigurationInterface* configuration, bool enabled); + explicit SensorDataSourceConfiguration(const ConfigurationInterface* configuration); bool validate() const; bool is_enabled() const; + bool is_sensor_provided(SensorIdentifier::value_type sensor_id) const; + const std::unordered_map& files() const; const std::vector& sensors() const; diff --git a/src/algorithms/signal_source/libs/sensor_data_type.cc b/src/algorithms/libs/sensor_data/sensor_data_type.cc similarity index 76% rename from src/algorithms/signal_source/libs/sensor_data_type.cc rename to src/algorithms/libs/sensor_data/sensor_data_type.cc index 6a30999ea..5d94b2e4c 100644 --- a/src/algorithms/signal_source/libs/sensor_data_type.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_type.cc @@ -15,14 +15,24 @@ */ #include "sensor_data_type.h" -#include #include +#include #include #include SensorDataType::value_type SensorDataType::from_string(const std::string& s) { - if (s == "float" or s == "FLOAT") + std::string str = s; + for (char& c : str) + { + c = toupper(c); + } + + if (str == "UINT64") + { + return SensorDataType::UINT64; + } + else if (str == "FLOAT") { return SensorDataType::FLOAT; } @@ -33,8 +43,10 @@ std::string SensorDataType::to_string(const SensorDataType::value_type& v) { switch (v) { + case SensorDataType::UINT64: + return "UINT64"; case SensorDataType::FLOAT: - return "float"; + return "FLOAT"; default: return "UNKNOWN SENSOR"; } @@ -44,6 +56,8 @@ uint64_t SensorDataType::get_size(const SensorDataType::value_type& v) { switch (v) { + case SensorDataType::UINT64: + return sizeof(uint64_t); case SensorDataType::FLOAT: return sizeof(float); default: @@ -55,6 +69,8 @@ pmt::pmt_t SensorDataType::make_value(const SensorDataType::value_type& v, void* { switch (v) { + case SensorDataType::UINT64: + return pmt::from_uint64(*static_cast(value)); case SensorDataType::FLOAT: return pmt::from_float(*static_cast(value)); default: diff --git a/src/algorithms/signal_source/libs/sensor_data_type.h b/src/algorithms/libs/sensor_data/sensor_data_type.h similarity index 98% rename from src/algorithms/signal_source/libs/sensor_data_type.h rename to src/algorithms/libs/sensor_data/sensor_data_type.h index ed5a6d7a4..645cc7901 100644 --- a/src/algorithms/signal_source/libs/sensor_data_type.h +++ b/src/algorithms/libs/sensor_data/sensor_data_type.h @@ -32,6 +32,7 @@ struct SensorDataType SensorDataType() = delete; enum value_type { + UINT64, FLOAT }; diff --git a/src/algorithms/signal_source/libs/sensor_identifier.cc b/src/algorithms/libs/sensor_data/sensor_identifier.cc similarity index 52% rename from src/algorithms/signal_source/libs/sensor_identifier.cc rename to src/algorithms/libs/sensor_data/sensor_identifier.cc index 2ba6719ff..aea7906a1 100644 --- a/src/algorithms/signal_source/libs/sensor_identifier.cc +++ b/src/algorithms/libs/sensor_data/sensor_identifier.cc @@ -15,12 +15,46 @@ */ #include "sensor_identifier.h" +#include "sensor_data_type.h" #include #include + +struct ConversionEntry +{ + SensorDataType::value_type from; + SensorDataType::value_type to; + pmt::pmt_t (*conversion_fun)(const pmt::pmt_t&); +}; + +static const ConversionEntry conversion_table[] = { + {SensorDataType::FLOAT, SensorDataType::FLOAT, [](const pmt::pmt_t& val) { return val; }}, +}; + +const ConversionEntry* lookup_conversion(SensorDataType::value_type from_type, SensorDataType::value_type to_type) +{ + for (const auto& conversion_entry : conversion_table) + { + if (conversion_entry.from == from_type && conversion_entry.to == to_type) + { + return &conversion_entry; + } + } + return nullptr; +} + + SensorIdentifier::value_type SensorIdentifier::from_string(const std::string& s) { - if (s == "IMU_VEL_X") + if (s == "SAMPLE_STAMP") + { + return SAMPLE_STAMP; + } + else if (s == "CHUNK_COUNT") + { + return CHUNK_COUNT; + } + else if (s == "IMU_VEL_X") { return IMU_VEL_X; } @@ -71,10 +105,14 @@ SensorIdentifier::value_type SensorIdentifier::from_string(const std::string& s) throw std::runtime_error{"Unknown sensor identifier: " + s}; } -std::string SensorIdentifier::to_string(const SensorIdentifier::value_type& v) +std::string SensorIdentifier::to_string(SensorIdentifier::value_type v) { switch (v) { + case SAMPLE_STAMP: + return "SAMPLE_STAMP"; + case CHUNK_COUNT: + return "CHUNK_COUNT"; case IMU_VEL_X: return "IMU_VEL_X"; case IMU_VEL_Y: @@ -103,3 +141,46 @@ std::string SensorIdentifier::to_string(const SensorIdentifier::value_type& v) return "UNKNOWN SENSOR"; } } + + +bool SensorIdentifier::is_valid_type(SensorIdentifier::value_type sensor_id, SensorDataType::value_type type) +{ + return nullptr != lookup_conversion(type, get_internal_type(sensor_id)); +} + + +SensorDataType::value_type SensorIdentifier::get_internal_type(value_type sensor_id) +{ + switch (sensor_id) + { + case SAMPLE_STAMP: + case CHUNK_COUNT: + return SensorDataType::UINT64; + case IMU_VEL_X: + case IMU_VEL_Y: + case IMU_VEL_Z: + case IMU_ACC_X: + case IMU_ACC_Y: + case IMU_ACC_Z: + case IMU_ANG_VEL_X: + case IMU_ANG_VEL_Y: + case IMU_ANG_VEL_Z: + case IMU_ANG_ACC_X: + case IMU_ANG_ACC_Y: + case IMU_ANG_ACC_Z: + return SensorDataType::FLOAT; + default: + return SensorDataType::FLOAT; + } +} + + +pmt::pmt_t SensorIdentifier::convert_to_internal_type(value_type sensor_id, SensorDataType::value_type original_type, const pmt::pmt_t& value) +{ + const ConversionEntry* conversion = lookup_conversion(original_type, get_internal_type(sensor_id)); + if (conversion == nullptr) + { + } + + return conversion->conversion_fun(value); +} diff --git a/src/algorithms/signal_source/libs/sensor_identifier.h b/src/algorithms/libs/sensor_data/sensor_identifier.h similarity index 71% rename from src/algorithms/signal_source/libs/sensor_identifier.h rename to src/algorithms/libs/sensor_data/sensor_identifier.h index 3eeee47be..e6776b3b9 100644 --- a/src/algorithms/signal_source/libs/sensor_identifier.h +++ b/src/algorithms/libs/sensor_data/sensor_identifier.h @@ -18,6 +18,7 @@ #ifndef GNSS_SDR_SENSOR_IDENTIFIER_H #define GNSS_SDR_SENSOR_IDENTIFIER_H +#include "sensor_data_type.h" #include /** \addtogroup Signal_Source @@ -28,8 +29,10 @@ struct SensorIdentifier { SensorIdentifier() = delete; - enum value_type + enum value_type : unsigned short { + SAMPLE_STAMP = 0, + CHUNK_COUNT, IMU_VEL_X, IMU_VEL_Y, IMU_VEL_Z, @@ -46,7 +49,13 @@ struct SensorIdentifier static value_type from_string(const std::string& s); - static std::string to_string(const value_type& v); + static std::string to_string(value_type v); + + static bool is_valid_type(value_type sensor_id, SensorDataType::value_type type); + + static SensorDataType::value_type get_internal_type(value_type sensor_id); + + static pmt::pmt_t convert_to_internal_type(value_type sensor_id, SensorDataType::value_type original_type, const pmt::pmt_t& value); }; /** \} */ diff --git a/src/algorithms/signal_source/adapters/file_source_base.cc b/src/algorithms/signal_source/adapters/file_source_base.cc index 0aff273ec..7cc30455f 100644 --- a/src/algorithms/signal_source/adapters/file_source_base.cc +++ b/src/algorithms/signal_source/adapters/file_source_base.cc @@ -61,7 +61,7 @@ FileSourceBase::FileSourceBase(ConfigurationInterface const* configuration, std: repeat_(configuration->property(role_ + ".repeat"s, false)), enable_throttle_control_(configuration->property(role_ + ".enable_throttle_control"s, false)), dump_(configuration->property(role_ + ".dump"s, false)), - sensor_data_source_configuration_(configuration, configuration->property(role_ + ".sensor_data.enabled"s, false)) + sensor_data_source_configuration_(configuration) { minimum_tail_s_ = std::max(configuration->property("Acquisition_1C.coherent_integration_time_ms", 0.0) * 0.001 * 2.0, minimum_tail_s_); minimum_tail_s_ = std::max(configuration->property("Acquisition_2S.coherent_integration_time_ms", 0.0) * 0.001 * 2.0, minimum_tail_s_); diff --git a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt index a031ab76e..8b429c6ed 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt @@ -20,6 +20,11 @@ if(ENABLE_ION) set(OPT_DRIVER_HEADERS ${OPT_DRIVER_HEADERS} ion_gsms.h) endif() +if (ENABLE_SENSOR_DATA) + set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} sensor_data_source.cc) + set(OPT_DRIVER_HEADERS ${OPT_DRIVER_HEADERS} sensor_data_source.h) +endif () + set(SIGNAL_SOURCE_GR_BLOCKS_SOURCES fifo_reader.cc unpack_byte_2bit_samples.cc @@ -31,7 +36,6 @@ set(SIGNAL_SOURCE_GR_BLOCKS_SOURCES unpack_2bit_samples.cc unpack_spir_gss6450_samples.cc labsat23_source.cc - sensor_data_source.cc ${OPT_DRIVER_SOURCES} ) @@ -46,7 +50,6 @@ set(SIGNAL_SOURCE_GR_BLOCKS_HEADERS unpack_2bit_samples.h unpack_spir_gss6450_samples.h labsat23_source.h - sensor_data_source.h ${OPT_DRIVER_HEADERS} ) diff --git a/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.cc b/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.cc index bf22cdf9c..7f1d2df05 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.cc @@ -15,7 +15,7 @@ */ #include "sensor_data_source.h" -#include "sensor_data_file.h" +#include "sensor_data/sensor_data_file.h" #include #if USE_GLOG_AND_GFLAGS @@ -83,15 +83,16 @@ int SensorDataSource::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items) { + static pmt::pmt_t TAG_KEY = pmt::mp("sensor_data"); + static pmt::pmt_t CHUNK_COUNT_KEY = pmt::mp("CHUNK_COUNT"); + static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp("SAMPLE_STAMP"); + std::memcpy(output_items[0], input_items[0], noutput_items * item_size_); const uint64_t total_items_written = nitems_written(0) + noutput_items; std::size_t sample_stamp; std::vector chunk{}; - pmt::pmt_t tag_key = pmt::mp("sensor_data"); - pmt::pmt_t chunk_count_key = pmt::mp("CHUNK_COUNT"); - pmt::pmt_t sample_stamp_key = pmt::mp("SAMPLE_STAMP"); for (auto& file_pair : sensor_data_files_) { const auto& file_id = file_pair.first; @@ -99,13 +100,13 @@ int SensorDataSource::work(int noutput_items, while (data_file->read_until_sample(total_items_written, sample_stamp, chunk)) { pmt::pmt_t data_tag = pmt::make_dict(); - data_tag = pmt::dict_add(data_tag, sample_stamp_key, pmt::from_long(sample_stamp / items_per_sample_)); - data_tag = pmt::dict_add(data_tag, chunk_count_key, pmt::from_long(data_file->get_chunks_read())); + data_tag = pmt::dict_add(data_tag, SAMPLE_STAMP_KEY, pmt::from_uint64(sample_stamp / items_per_sample_)); + data_tag = pmt::dict_add(data_tag, CHUNK_COUNT_KEY, pmt::from_long(data_file->get_chunks_read())); for (const auto& sensor : sensor_config_map_.at(file_id)) { data_tag = pmt::dict_add(data_tag, sensor.tag_key, SensorDataType::make_value(sensor.type, &chunk[sensor.offset])); } - add_item_tag(0, sample_stamp, tag_key, data_tag); + add_item_tag(0, sample_stamp, TAG_KEY, data_tag); } } diff --git a/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.h b/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.h index aed6fcae0..a99433588 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.h +++ b/src/algorithms/signal_source/gnuradio_blocks/sensor_data_source.h @@ -18,9 +18,9 @@ #ifndef GNSS_SDR_SENSOR_DATA_SOURCE_H #define GNSS_SDR_SENSOR_DATA_SOURCE_H +#include "sensor_data/sensor_data_file.h" +#include "sensor_data/sensor_data_source_configuration.h" #include "gnss_block_interface.h" -#include "sensor_data_file.h" -#include "sensor_data_source_configuration.h" #include // for sync_block #include // for gr_vector_const_void_star #include // for size_t @@ -52,57 +52,6 @@ private: std::size_t items_per_sample_; }; -class SensorDataAggregator -{ -public: - explicit SensorDataAggregator(std::vector tags) - { - for (const auto& sensor_tag : tags) - { - if (sensor_tag.value->is_dict()) - { - append_data(sensor_tag.value); - } - } - } - - auto get(SensorIdentifier::value_type sensor_id) const - { - if (data_.contains(sensor_id)) - { - return data_.at(sensor_id); - } - else - { - return {}; - } - } - -private: - void append_data(const pmt::pmt_t& data) - { - pmt::pmt_t data_list = pmt::dict_items(data); - while (not pmt::is_null(data_list)) - { - pmt::pmt_t pair = pmt::car(data_list); - pmt::pmt_t key = pmt::car(pair); - pmt::pmt_t val = pmt::cdr(pair); - - std::string key_str = pmt::write_string(key); - SensorIdentifier::value_type sensor_id = SensorIdentifier::from_string(key_str); - - if (not data_.contains(sensor_id)) - { - data_[sensor_id] = {}; - } - data_[sensor_id].emplace_back(val); - } - } - - std::unordered_map> data_{}; -}; - - /** \} */ /** \} */ #endif // GNSS_SDR_SENSOR_DATA_SOURCE_H diff --git a/src/algorithms/signal_source/libs/CMakeLists.txt b/src/algorithms/signal_source/libs/CMakeLists.txt index 158be92d4..0df4f3303 100644 --- a/src/algorithms/signal_source/libs/CMakeLists.txt +++ b/src/algorithms/signal_source/libs/CMakeLists.txt @@ -56,10 +56,6 @@ set(SIGNAL_SOURCE_LIB_SOURCES rtl_tcp_dongle_info.cc gnss_sdr_valve.cc gnss_sdr_timestamp.cc - sensor_data_file.cc - sensor_data_source_configuration.cc - sensor_data_type.cc - sensor_identifier.cc ${OPT_SIGNAL_SOURCE_LIB_SOURCES} ) @@ -67,10 +63,6 @@ set(SIGNAL_SOURCE_LIB_HEADERS rtl_tcp_commands.h rtl_tcp_dongle_info.h gnss_sdr_valve.h - sensor_data_file.h - sensor_data_source_configuration.h - sensor_data_type.h - sensor_identifier.h ${OPT_SIGNAL_SOURCE_LIB_HEADERS} )