From 42c2e3c2deb3d037e2af4dbf56f987ad2b966816 Mon Sep 17 00:00:00 2001 From: Victor Castillo Date: Mon, 29 Jul 2024 00:26:50 +0200 Subject: [PATCH] Added extra data source which can be connected after a Signal Source in order to attach extra data (such as IMU measurements) to the sample stream. --- .../gnuradio_blocks/CMakeLists.txt | 2 + .../gnuradio_blocks/extra_data_source.cc | 82 ++++++++++++++++ .../gnuradio_blocks/extra_data_source.h | 72 ++++++++++++++ .../signal_source/libs/CMakeLists.txt | 2 + .../signal_source/libs/extra_data_file.cc | 93 +++++++++++++++++++ .../signal_source/libs/extra_data_file.h | 69 ++++++++++++++ 6 files changed, 320 insertions(+) create mode 100644 src/algorithms/signal_source/gnuradio_blocks/extra_data_source.cc create mode 100644 src/algorithms/signal_source/gnuradio_blocks/extra_data_source.h create mode 100644 src/algorithms/signal_source/libs/extra_data_file.cc create mode 100644 src/algorithms/signal_source/libs/extra_data_file.h diff --git a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt index 24f067be6..81b01b89a 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt @@ -32,6 +32,7 @@ set(SIGNAL_SOURCE_GR_BLOCKS_SOURCES unpack_2bit_samples.cc unpack_spir_gss6450_samples.cc labsat23_source.cc + extra_data_source.cc ${OPT_DRIVER_SOURCES} ) @@ -46,6 +47,7 @@ set(SIGNAL_SOURCE_GR_BLOCKS_HEADERS unpack_2bit_samples.h unpack_spir_gss6450_samples.h labsat23_source.h + extra_data_source.h ${OPT_DRIVER_HEADERS} ) diff --git a/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.cc b/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.cc new file mode 100644 index 000000000..ec75b0111 --- /dev/null +++ b/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.cc @@ -0,0 +1,82 @@ +/*! + * \file extra_data_source.cc + * \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 + * + * ----------------------------------------------------------------------------- + */ + +#include "extra_data_source.h" +#include + + +ExtraDataSource::ExtraDataSource( + const std::string& path, + const std::size_t& offset_in_file, + const std::size_t& item_size, + const bool& repeat, + const std::size_t& offset_in_samples, + const std::size_t& sample_period, + const gr::io_signature::sptr& io_signature + ) + : gr::sync_block("Extra Data Source", + io_signature, io_signature), + extra_data_file_( + path, + offset_in_file, + item_size, + repeat), + offset_in_samples_(offset_in_samples), + sample_period_(sample_period), + last_tagged_sample_(offset_in_samples_) +{ + if (io_signature->min_streams() != 1 and io_signature->max_streams() != 1) + { + std::cout << "ERROR: This block only supports adding data to a single stream." << "\n"; + } +} + +std::vector ExtraDataSource::get_next_item() +{ + return extra_data_file_.read_item(); +} + +std::size_t ExtraDataSource::get_offset_in_samples() const +{ + return offset_in_samples_; +} + +std::size_t ExtraDataSource::get_sample_period() const +{ + return sample_period_; +} + +int ExtraDataSource::work(int noutput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items) override +{ + const std::size_t ch = 0; + const int item_size = input_signature()->sizeof_stream_item(ch); + std::memcpy(output_items[ch], input_items[ch], noutput_items * item_size); + + const uint64_t total_items_written = nitems_written(ch); + if (total_items_written >= sample_period_ + last_tagged_sample_) + { + for (uint64_t sample = last_tagged_sample_ + sample_period_; sample < total_items_written; sample += sample_period_) + { + auto extra_data_item = get_next_item(); + add_item_tag(ch, sample, pmt::mp("extra_data"), pmt::init_u8vector(extra_data_item.size(), extra_data_item)); + last_tagged_sample_ = sample; + } + } + + return noutput_items; +} diff --git a/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.h b/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.h new file mode 100644 index 000000000..7c20dbd69 --- /dev/null +++ b/src/algorithms/signal_source/gnuradio_blocks/extra_data_source.h @@ -0,0 +1,72 @@ +/*! + * \file extra_data_source.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_EXTRA_DATA_SOURCE_H +#define GNSS_SDR_EXTRA_DATA_SOURCE_H + +#include "gnss_block_interface.h" +#include "extra_data_file.h" +#include // for sync_block +#include // for gr_vector_const_void_star +#include // for size_t +#include +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_gnuradio_blocks + * \{ */ + + +class ExtraDataSource: public gr::sync_block +{ +public: + using sptr = gnss_shared_ptr; + + ExtraDataSource( + const std::string& path, + const std::size_t& offset_in_file, + const std::size_t& item_size, + const bool& repeat, + const std::size_t& offset_in_samples, + const std::size_t& sample_period, + const gr::io_signature::sptr& io_signature); + +private: + std::vector get_next_item(); + + std::size_t get_offset_in_samples() const; + + std::size_t get_sample_period() const; + +public: + int work(int noutput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items) override; + +private: + ExtraDataFile extra_data_file_; + std::size_t offset_in_samples_; + std::size_t sample_period_; + + std::size_t last_tagged_sample_; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_EXTRA_DATA_SOURCE_H diff --git a/src/algorithms/signal_source/libs/CMakeLists.txt b/src/algorithms/signal_source/libs/CMakeLists.txt index 3506dc4f7..03476a79d 100644 --- a/src/algorithms/signal_source/libs/CMakeLists.txt +++ b/src/algorithms/signal_source/libs/CMakeLists.txt @@ -50,6 +50,7 @@ set(SIGNAL_SOURCE_LIB_SOURCES rtl_tcp_dongle_info.cc gnss_sdr_valve.cc gnss_sdr_timestamp.cc + extra_data_file.cc ${OPT_SIGNAL_SOURCE_LIB_SOURCES} ) @@ -57,6 +58,7 @@ set(SIGNAL_SOURCE_LIB_HEADERS rtl_tcp_commands.h rtl_tcp_dongle_info.h gnss_sdr_valve.h + extra_data_file.h ${OPT_SIGNAL_SOURCE_LIB_HEADERS} ) diff --git a/src/algorithms/signal_source/libs/extra_data_file.cc b/src/algorithms/signal_source/libs/extra_data_file.cc new file mode 100644 index 000000000..9428908c6 --- /dev/null +++ b/src/algorithms/signal_source/libs/extra_data_file.cc @@ -0,0 +1,93 @@ +/*! + * \file extra_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 "extra_data_file.h" +#include + +ExtraDataFile::ExtraDataFile( + const std::string& path, + const std::size_t& offset_in_file, + const std::size_t& item_size, + const bool& repeat) + : path_(path), + file_(path_), + offset_in_file_(offset_in_file), + item_size_(item_size), + repeat_(repeat), + done_(false), + io_buffer_size_(item_size * IO_BUFFER_CAPACITY), + offset_in_io_buffer_(io_buffer_size_) // Set to end of buffer so that first look up will trigger a read. +{ + file_.seekg(offset_in_file_, std::ios_base::beg); + + io_buffer_.resize(io_buffer_size_); +} + +void ExtraDataFile::reset() +{ + file_.seekg(offset_in_file_, std::ios_base::beg); + offset_in_io_buffer_ = io_buffer_size_; + done_ = false; +} + +std::vector ExtraDataFile::read_item() +{ + if (offset_in_io_buffer_ >= io_buffer_size_) + { + if (done_) + { + return {}; + } + else + { + read_into_io_buffer(); + } + } + + std::vector item_buf{}; + read_into_item_buffer(item_buf); + + return item_buf; +} + +void ExtraDataFile::read_into_io_buffer() +{ + file_.get(reinterpret_cast(io_buffer_.data()), io_buffer_size_); + const std::size_t bytes_read = file_.gcount(); + + if (bytes_read < io_buffer_size_) + { + if (repeat_) + { + reset(); + file_.get(reinterpret_cast(&io_buffer_[bytes_read]), io_buffer_size_ - bytes_read); + } + else + { + io_buffer_size_ = bytes_read; + done_ = true; + } + } + + offset_in_io_buffer_ = 0; +} + +void ExtraDataFile::read_into_item_buffer(std::vector& item_buf) +{ + item_buf.resize(item_size_); + std::memcpy(item_buf.data(), &io_buffer_[offset_in_io_buffer_], item_size_); + offset_in_io_buffer_ += item_size_; +} diff --git a/src/algorithms/signal_source/libs/extra_data_file.h b/src/algorithms/signal_source/libs/extra_data_file.h new file mode 100644 index 000000000..b19a719e5 --- /dev/null +++ b/src/algorithms/signal_source/libs/extra_data_file.h @@ -0,0 +1,69 @@ +/*! + * \file extra_data_file.h + * \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 + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_EXTRA_DATA_FILE_H +#define GNSS_SDR_EXTRA_DATA_FILE_H + +#include // for size_t +#include +#include +#include +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_libs + * \{ */ + + +class ExtraDataFile +{ + static constexpr std::size_t IO_BUFFER_CAPACITY = 1024; + +public: + ExtraDataFile( + const std::string& path, + const std::size_t& offset_in_file, + const std::size_t& item_size, + const bool& repeat); + + + void reset(); + + std::vector read_item(); + +private: + void read_into_io_buffer(); + + void read_into_item_buffer(std::vector& item_buf); + +private: + std::string path_; + std::ifstream file_; + std::size_t offset_in_file_; + std::size_t item_size_; + bool repeat_; + bool done_; + + std::vector io_buffer_; + std::size_t io_buffer_size_; + std::size_t offset_in_io_buffer_; +}; + +/** \} */ +/** \} */ +#endif // GNSS_SDR_EXTRA_DATA_FILE_H