From 878df300506a7287862f9195ec3275dca47ef2e4 Mon Sep 17 00:00:00 2001 From: Victor Castillo Date: Fri, 29 Aug 2025 20:52:58 +0200 Subject: [PATCH] fix(sensor_data): update sensor data tag timestamp in resampler block --- src/algorithms/libs/CMakeLists.txt | 2 + .../libs/sensor_data/sensor_data_resampler.cc | 37 +++++++++++++++++++ .../libs/sensor_data/sensor_data_resampler.h | 35 ++++++++++++++++++ .../gnuradio_blocks/hybrid_observables_gs.cc | 10 ++++- .../gnuradio_blocks/hybrid_observables_gs.h | 2 +- .../direct_resampler_conditioner_cb.cc | 15 ++++++++ .../direct_resampler_conditioner_cc.cc | 15 ++++++++ .../direct_resampler_conditioner_cs.cc | 15 ++++++++ 8 files changed, 129 insertions(+), 2 deletions(-) create mode 100644 src/algorithms/libs/sensor_data/sensor_data_resampler.cc create mode 100644 src/algorithms/libs/sensor_data/sensor_data_resampler.h diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index bf46193d3..b537cd59b 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -74,6 +74,7 @@ set(GNSS_SPLIBS_HEADERS ${GNSS_SPLIBS_HEADERS} sensor_data/sensor_data_source_configuration.h sensor_data/sensor_data_source.h sensor_data/sensor_data_aggregator.h + sensor_data/sensor_data_resampler.h ) set(GNSS_SPLIBS_SOURCES ${GNSS_SPLIBS_SOURCES} sensor_data/sensor_data_type.cc @@ -82,6 +83,7 @@ set(GNSS_SPLIBS_SOURCES ${GNSS_SPLIBS_SOURCES} sensor_data/sensor_data_source_configuration.cc sensor_data/sensor_data_source.cc sensor_data/sensor_data_aggregator.cc + sensor_data/sensor_data_resampler.cc ) if(ENABLE_OPENCL) diff --git a/src/algorithms/libs/sensor_data/sensor_data_resampler.cc b/src/algorithms/libs/sensor_data/sensor_data_resampler.cc new file mode 100644 index 000000000..2f6a86956 --- /dev/null +++ b/src/algorithms/libs/sensor_data/sensor_data_resampler.cc @@ -0,0 +1,37 @@ +/*! + * \file sensor_data_resampler.cc + * \brief Updates timestamp within sensor data tags. To be used within resampler blocks. + * \author Victor Castillo, 2024. victorcastilloaguero(at)gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2025 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "sensor_data_resampler.h" +#include "sensor_identifier.h" + +std::vector resample_sensor_data_tags(const std::vector& tags, double freq_in, double freq_out) +{ + static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP)); + std::vector new_tags{}; + for (auto& tag : tags) + { + if (pmt::dict_has_key(tag.value, SAMPLE_STAMP_KEY)) + { + auto& new_tag = new_tags.emplace_back(); + uint64_t sample_stamp = pmt::to_uint64(pmt::dict_ref(tag.value, SAMPLE_STAMP_KEY, pmt::from_uint64(0))); + sample_stamp = sample_stamp * freq_out / freq_in; + new_tag.offset = tag.offset * freq_out / freq_in; + new_tag.key = tag.key; + new_tag.value = pmt::dict_add(tag.value, SAMPLE_STAMP_KEY, pmt::from_uint64(sample_stamp)); + } + } + return new_tags; +} diff --git a/src/algorithms/libs/sensor_data/sensor_data_resampler.h b/src/algorithms/libs/sensor_data/sensor_data_resampler.h new file mode 100644 index 000000000..8ca4dcb7d --- /dev/null +++ b/src/algorithms/libs/sensor_data/sensor_data_resampler.h @@ -0,0 +1,35 @@ +/*! + * \file sensor_data_resampler.h + * \brief Updates timestamp within sensor data tags. To be used within resampler blocks. + * \author Victor Castillo, 2024. victorcastilloaguero(at)gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2025 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_SENSOR_DATA_RESAMPLER_H +#define GNSS_SDR_SENSOR_DATA_RESAMPLER_H + +#include +#include + +/** \addtogroup Algorithms_Library + * \{ */ +/** \addtogroup Algorithm_libs algorithms_libs + * \{ */ + + +std::vector resample_sensor_data_tags(const std::vector& tags, double freq_in, double freq_out); + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_SENSOR_DATA_RESAMPLER_H diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 2e526b85b..bd5b3a6f5 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -661,7 +661,15 @@ void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(const std::vector void hybrid_observables_gs::propagate_sensor_data(const std::vector &data) { static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP)); - uint64_t current_sample = data[0].Tracking_sample_counter * 10; + uint64_t current_sample = 0; + for (const Gnss_Synchro& item: data) + { + if (item.Tracking_sample_counter > current_sample) + { + current_sample = item.Tracking_sample_counter; + } + } + if (d_trq_last_sample == 0) { d_trq_last_sample = current_sample; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index b237623fd..8e20cb5e9 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -94,7 +94,7 @@ private: std::queue d_TimeChannelTagTimestamps; std::queue d_sensor_data_tags; - std::uint64_t d_trq_last_sample; + std::uint64_t d_trq_last_sample{0}; std::vector d_channel_last_pll_lock; std::vector d_channel_last_pseudorange_smooth; diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.cc b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.cc index e0221ecc3..4ac46b77d 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.cc +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cb.cc @@ -18,6 +18,8 @@ #include "direct_resampler_conditioner_cb.h" +#include "../../libs/sensor_data/sensor_data_resampler.h" +#include "../../libs/sensor_data/sensor_identifier.h" #include #include // for lv_8sc_t #include // for min @@ -58,6 +60,10 @@ direct_resampler_conditioner_cb::direct_resampler_conditioner_cb( #else this->set_relative_rate(sample_freq_out / sample_freq_in); #endif + + // Do not propagate tags, they carry a sample offset that is not properly updated in + // this resampler and thus needs manual handling. + this->set_tag_propagation_policy(TPP_DONT); } @@ -111,6 +117,15 @@ int direct_resampler_conditioner_cb::general_work(int noutput_items, } } + // Sensor data tag resampling + std::vector sensor_tags; + this->get_tags_in_window(sensor_tags, 0, 0, std::min(count, ninput_items[0]), pmt::mp("sensor_data")); + for (auto &tag : resample_sensor_data_tags(sensor_tags, d_sample_freq_in, d_sample_freq_out)) + { + this->add_item_tag(0, tag); + } + + consume_each(std::min(count, ninput_items[0])); return lcv; } diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.cc b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.cc index d3a9544a7..a46333a87 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.cc +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.cc @@ -18,6 +18,8 @@ #include "direct_resampler_conditioner_cc.h" +#include "../../libs/sensor_data/sensor_data_resampler.h" +#include "../../libs/sensor_data/sensor_identifier.h" #include #include // for gr_complex #include // for min @@ -57,6 +59,10 @@ direct_resampler_conditioner_cc::direct_resampler_conditioner_cc( #else this->set_relative_rate(sample_freq_out / sample_freq_in); #endif + + // Do not propagate tags, they carry a sample offset that is not properly updated in + // this resampler and thus needs manual handling. + this->set_tag_propagation_policy(TPP_DONT); } @@ -109,6 +115,15 @@ int direct_resampler_conditioner_cc::general_work(int noutput_items, } } + // Sensor data tag resampling + std::vector sensor_tags; + this->get_tags_in_window(sensor_tags, 0, 0, std::min(count, ninput_items[0]), pmt::mp("sensor_data")); + for (auto &tag : resample_sensor_data_tags(sensor_tags, d_sample_freq_in, d_sample_freq_out)) + { + this->add_item_tag(0, tag); + } + + consume_each(std::min(count, ninput_items[0])); return lcv; } diff --git a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.cc b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.cc index 8f8407343..69c85accc 100644 --- a/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.cc +++ b/src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cs.cc @@ -18,6 +18,8 @@ #include "direct_resampler_conditioner_cs.h" +#include "../../libs/sensor_data/sensor_data_resampler.h" +#include "../../libs/sensor_data/sensor_identifier.h" #include #include // for lv_16sc_t #include // for min @@ -59,6 +61,10 @@ direct_resampler_conditioner_cs::direct_resampler_conditioner_cs( #else this->set_relative_rate(sample_freq_out / sample_freq_in); #endif + + // Do not propagate tags, they carry a sample offset that is not properly updated in + // this resampler and thus needs manual handling. + this->set_tag_propagation_policy(TPP_DONT); } @@ -112,6 +118,15 @@ int direct_resampler_conditioner_cs::general_work(int noutput_items, } } + // Sensor data tag resampling + std::vector sensor_tags; + this->get_tags_in_window(sensor_tags, 0, 0, std::min(count, ninput_items[0]), pmt::mp("sensor_data")); + for (auto &tag : resample_sensor_data_tags(sensor_tags, d_sample_freq_in, d_sample_freq_out)) + { + this->add_item_tag(0, tag); + } + + consume_each(std::min(count, ninput_items[0])); return lcv; }