From 5c3134dbde791a09a88dc4f96dcab91016d32e37 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 19 Apr 2021 12:27:02 +0200 Subject: [PATCH] Move External TimeTag propagation to the time counter channel. PVT OBS timestamp comarison completed --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 71 +++++++- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 10 +- .../gnuradio_blocks/hybrid_observables_gs.cc | 157 ++++++++++++++---- .../gnuradio_blocks/hybrid_observables_gs.h | 2 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 34 ++-- src/core/libs/CMakeLists.txt | 1 + src/core/libs/gnss_sdr_sample_counter.cc | 45 +++++ src/core/libs/gnss_sdr_sample_counter.h | 2 + 8 files changed, 266 insertions(+), 56 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index a549750ee..f7714a6be 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1816,6 +1816,35 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset() int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { + //**************** time tags **************** + if (d_enable_rx_clock_correction == false) //todo: currently only works if clock correction is disabled + { + std::vector tags_vec; + //time tag from obs to pvt is always propagated in channel 0 + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items); + for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + { + try + { + if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); + //std::cout << "PVT timetag: " << timetag->rx_time << "\n"; + d_TimeChannelTagTimestamps.push(*timetag); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast& e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + } + //************* end time tags ************** + for (int32_t epoch = 0; epoch < noutput_items; epoch++) { bool flag_display_pvt = false; @@ -1999,7 +2028,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { flag_compute_pvt_output = true; // std::cout.precision(17); - // std::cout << "current_RX_time: " << current_RX_time << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n'; + //std::cout << "current_RX_time: " << current_RX_time_ms << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n'; } flag_pvt_valid = true; } @@ -2017,20 +2046,52 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false); } + if (flag_pvt_valid == true) { //experimental VTL tests // send tracking command - const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); - trk_cmd_test->carrier_freq_hz = 12345.4; - trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter; - this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); // initialize (if needed) the accumulated phase offset and apply it to the active channels // required to report accumulated phase cycles comparable to pseudoranges initialize_and_apply_carrier_phase_offset(); const double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s(); + + //**************** time tags **************** + if (d_enable_rx_clock_correction == false) //todo: currently only works if clock correction is disabled + { + //************ Source TimeTag comparison with GNSS computed TOW ************* + + if (!d_TimeChannelTagTimestamps.empty()) + { + double delta_rxtime_to_tag; + GnssTime current_tag; + do + { + current_tag = d_TimeChannelTagTimestamps.front(); + delta_rxtime_to_tag = d_rx_time * 1000.0 - current_tag.rx_time; + d_TimeChannelTagTimestamps.pop(); + } + while (fabs(delta_rxtime_to_tag) >= 100 and !d_TimeChannelTagTimestamps.empty()); + + + if (fabs(delta_rxtime_to_tag) <= 100) //[ms] + { + double timestamp_tow_error_ns = 1000000.0 * (Rx_clock_offset_s * 1000.0 + delta_rxtime_to_tag + static_cast(current_tag.tow_ms) - d_rx_time * 1000.0 + current_tag.tow_ms_fraction); + std::cout << "[Time ch] RX TimeTag Week: " << current_tag.week + << ", TOW: " << current_tag.tow_ms + << " [ms], TOW fraction: " << current_tag.tow_ms_fraction + << " [ms], GNSS-SDR OBS CORRECTED TOW - EXTERNAL TIMETAG TOW: " << timestamp_tow_error_ns << " [ns] \n"; + } + } + } + //********************************************** + if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !! { LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]" diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 4e6fd3144..c3f95148e 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -19,6 +19,7 @@ #include "gnss_block_interface.h" #include "gnss_synchro.h" +#include "gnss_time.h" #include "rtklib.h" #include #include @@ -31,9 +32,10 @@ #include // for time_t #include // for map #include // for shared_ptr, unique_ptr -#include // for string -#include // for key_t -#include // for vector +#include +#include // for string +#include // for key_t +#include // for vector /** \addtogroup PVT * \{ */ @@ -202,6 +204,8 @@ private: std::map d_gnss_observables_map_t0; std::map d_gnss_observables_map_t1; + std::queue d_TimeChannelTagTimestamps; + boost::posix_time::time_duration d_utc_diff_time; size_t d_gps_ephemeris_sptr_type_hash_code; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 72f8389c6..ced16e382 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -143,6 +143,8 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block( d_mapStringValues["B1"] = evBDS_B1; d_mapStringValues["B2"] = evBDS_B2; d_mapStringValues["B3"] = evBDS_B3; + last_rx_clock_round20ms_error = 0; + set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -192,12 +194,16 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg if (pmt::any_ref(msg).type().hash_code() == d_double_type_hash_code) { const auto new_rx_clock_offset_s = boost::any_cast(pmt::any_ref(msg)); + double old_tow_corrected = static_cast(d_T_rx_TOW_ms) - new_rx_clock_offset_s * 1000.0; + d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast(round(new_rx_clock_offset_s * 1000.0)); + // align the receiver clock to integer multiple of 20 ms if (d_T_rx_TOW_ms % 20) { d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20; } + last_rx_clock_round20ms_error = static_cast(d_T_rx_TOW_ms) - old_tow_corrected; // d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer for (uint32_t n = 0; n < d_nchannels_out; n++) { @@ -605,32 +611,93 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector &data) void hybrid_observables_gs::check_tag_timestamp(const std::vector &data, uint64_t rx_clock) { - std::vector::const_iterator it; - for (it = data.begin(); it != data.end(); it++) - { - if (!d_SourceTagTimestamps[it->Channel_ID].empty() and it->Flag_valid_pseudorange == true) - { - //std::cout << "RX Time: " << (static_cast(rx_clock) / static_cast(it->fs)) << "s\n"; - double delta_rxtime_to_tag; - GnssTime current_tag; - do - { - current_tag = d_SourceTagTimestamps[it->Channel_ID].front(); - delta_rxtime_to_tag = (static_cast(rx_clock) / static_cast(it->fs)) - current_tag.rx_time; - // std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag << "]\n"; - d_SourceTagTimestamps[it->Channel_ID].pop(); - } - while (fabs(delta_rxtime_to_tag) >= 0.05 and !d_SourceTagTimestamps[it->Channel_ID].empty()); + // std::vector::const_iterator it; + // for (it = data.begin(); it != data.end(); it++) + // { + // if (!d_SourceTagTimestamps[it->Channel_ID].empty() and it->Flag_valid_pseudorange == true) + // { + // //std::cout << "RX Time: " << (static_cast(rx_clock) / static_cast(it->fs)) << "s\n"; + // double delta_rxtime_to_tag; + // GnssTime current_tag; + // do + // { + // current_tag = d_SourceTagTimestamps[it->Channel_ID].front(); + // delta_rxtime_to_tag = (static_cast(rx_clock) / static_cast(it->fs)) - current_tag.rx_time; + // // std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag << "]\n"; + // d_SourceTagTimestamps[it->Channel_ID].pop(); + // } + // while (fabs(delta_rxtime_to_tag) >= 0.2 and !d_SourceTagTimestamps[it->Channel_ID].empty()); + // + // if (fabs(delta_rxtime_to_tag) <= 0.2) + // { + // std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag + // << "] OBS RX TimeTag Week: " << current_tag.week + // << ", TOW: " << current_tag.tow_ms + // << " [ms], TOW fraction: " << current_tag.tow_ms_fraction + // << " [ms], DELTA TLM TOW: " << last_rx_clock_round20ms_error + delta_rxtime_to_tag * 1000.0 + static_cast(current_tag.tow_ms) - static_cast(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n"; + // + // const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + // *tmp_obj = current_tag; + // tmp_obj->week = current_tag.week; + // double intpart; + // tmp_obj->tow_ms_fraction = modf(delta_rxtime_to_tag * 1000.0, &intpart); + // tmp_obj->tow_ms = current_tag.tow_ms + static_cast(intpart); + // tmp_obj->rx_time = static_cast(rx_clock) / static_cast(it->fs); + // add_item_tag(it->Channel_ID, this->nitems_written(it->Channel_ID) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + // } + // } + // } - if (fabs(delta_rxtime_to_tag) <= 0.05) + //std::cout << "RX Time: " << (static_cast(rx_clock) / static_cast(it->fs)) << "s\n"; + + if (!d_TimeChannelTagTimestamps.empty()) + { + double fs = 0; + std::vector::const_iterator it; + for (it = data.begin(); it != data.end(); it++) + { + if (it->Flag_valid_pseudorange == true) { - std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag - << "] OBS RX TimeTag Week: " << current_tag.week - << ", TOW: " << current_tag.tow_ms - << " [ms], TOW fraction: " << current_tag.tow_ms_fraction - << " [ms], DELTA TLM TOW: " << delta_rxtime_to_tag * 1000.0 + static_cast(current_tag.tow_ms) - it->RX_time * 1000.0 + current_tag.tow_ms_fraction << " [ms] \n"; + fs = static_cast(it->fs); + break; } } + + double delta_rxtime_to_tag = 100; + GnssTime current_tag; + do + { + current_tag = d_TimeChannelTagTimestamps.front(); + delta_rxtime_to_tag = (static_cast(rx_clock) / fs) - current_tag.rx_time; + if (delta_rxtime_to_tag >= 0) + { + d_TimeChannelTagTimestamps.pop(); + } + } + while (delta_rxtime_to_tag >= 0.1 and !d_TimeChannelTagTimestamps.empty()); + + + if (delta_rxtime_to_tag >= 0 and delta_rxtime_to_tag <= 0.1) + { + // std::cout << "[Time ch][" << delta_rxtime_to_tag + // << "] OBS RX TimeTag Week: " << current_tag.week + // << ", TOW: " << current_tag.tow_ms + // << " [ms], TOW fraction: " << current_tag.tow_ms_fraction + // << " [ms], DELTA TLM TOW: " << last_rx_clock_round20ms_error + delta_rxtime_to_tag * 1000.0 + static_cast(current_tag.tow_ms) - static_cast(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n"; + + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + *tmp_obj = current_tag; + double intpart; + tmp_obj->tow_ms_fraction = tmp_obj->tow_ms_fraction + modf(delta_rxtime_to_tag * 1000.0, &intpart); + tmp_obj->tow_ms = current_tag.tow_ms + static_cast(intpart); + tmp_obj->rx_time = static_cast(d_T_rx_TOW_ms); //static_cast(rx_clock) / static_cast(data.begin()->fs); + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + delta_rxtime_to_tag = 100; + } + // else + // { + // std::cout << "Delta: " << delta_rxtime_to_tag << "\n"; + // } } } @@ -647,16 +714,10 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) if (ninput_items[d_nchannels_in - 1] > 0) { d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter); - // Consume one item from the clock channel (last of the input channels) - consume(static_cast(d_nchannels_in) - 1, 1); - } - // Push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock - for (uint32_t n = 0; n < d_nchannels_out; n++) - { //**************** time tags **************** std::vector tags_vec; - this->get_tags_in_range(tags_vec, n, this->nitems_read(n), this->nitems_read(n) + ninput_items[n]); + this->get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1); for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) { try @@ -664,8 +725,8 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); - //std::cout << "[ch " << n << "] timetag: " << timetag->rx_time << "\n"; - d_SourceTagTimestamps.at(n).push(*timetag); + //std::cout << "[Time ch ] timetag: " << timetag->rx_time << "\n"; + d_TimeChannelTagTimestamps.push(*timetag); } else { @@ -679,6 +740,40 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) } //************* end time tags ************** + + + // Consume one item from the clock channel (last of the input channels) + consume(static_cast(d_nchannels_in) - 1, 1); + } + + // Push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock + for (uint32_t n = 0; n < d_nchannels_out; n++) + { + //**************** time tags **************** + // std::vector tags_vec; + // this->get_tags_in_range(tags_vec, n, this->nitems_read(n), this->nitems_read(n) + ninput_items[n]); + // for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + // { + // try + // { + // if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + // { + // const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); + // //std::cout << "[ch " << n << "] timetag: " << timetag->rx_time << "\n"; + // d_SourceTagTimestamps.at(n).push(*timetag); + // } + // else + // { + // std::cout << "hash code not match\n"; + // } + // } + // catch (const boost::bad_any_cast &e) + // { + // std::cout << "msg Bad any_cast: " << e.what(); + // } + // } + + //************* end time tags ************** for (int32_t m = 0; m < ninput_items[n]; m++) { // Push the valid tracking Gnss_Synchros to their corresponding deque diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 0530f7c87..4fa973de2 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -106,6 +106,7 @@ private: boost::circular_buffer d_Rx_clock_buffer; // time history std::vector> d_SourceTagTimestamps; + std::queue d_TimeChannelTagTimestamps; std::vector d_channel_last_pll_lock; std::vector d_channel_last_pseudorange_smooth; @@ -118,6 +119,7 @@ private: double d_smooth_filter_M; uint32_t d_T_rx_TOW_ms; + double last_rx_clock_round20ms_error; uint32_t d_T_rx_step_ms; uint32_t d_T_status_report_timer_ms; uint32_t d_nchannels_in; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index fd4d7776f..611ae4d4b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -2036,23 +2036,23 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) //generate new tag associated with gnss-synchro object - if (d_timetag_waiting == true) - { - int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); - - double intpart; - d_last_timetag.tow_ms_fraction = modf(1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in, &intpart); - - const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); - tmp_obj->week = d_last_timetag.week; - tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast(intpart); - tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction; - tmp_obj->rx_time = static_cast(current_synchro_data.Tracking_sample_counter) / d_trk_parameters.fs_in; - add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); - - //std::cout << "[" << this->nitems_written(0) + 1 << "] Sent TimeTag Week: " << d_last_timetag.week << ", TOW: " << d_last_timetag.tow_ms << " [ms], TOW fraction: " << d_last_timetag.tow_ms_fraction << " [ms] \n"; - d_timetag_waiting = false; - } + // if (d_timetag_waiting == true) + // { + // int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); + // + // double intpart; + // d_last_timetag.tow_ms_fraction = modf(1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in, &intpart); + // + // const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + // tmp_obj->week = d_last_timetag.week; + // tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast(intpart); + // tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction; + // tmp_obj->rx_time = static_cast(current_synchro_data.Tracking_sample_counter) / d_trk_parameters.fs_in; + // add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + // + // //std::cout << "[" << this->nitems_written(0) + 1 << "][diff_time: " << 1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in << "] Sent TimeTag Week: " << d_last_timetag.week << ", TOW: " << d_last_timetag.tow_ms << " [ms], TOW fraction: " << d_last_timetag.tow_ms_fraction << " [ms] \n"; + // d_timetag_waiting = false; + // } return 1; } diff --git a/src/core/libs/CMakeLists.txt b/src/core/libs/CMakeLists.txt index a525f87c4..d89f82a43 100644 --- a/src/core/libs/CMakeLists.txt +++ b/src/core/libs/CMakeLists.txt @@ -76,6 +76,7 @@ target_link_libraries(core_libs core_libs_supl core_system_parameters pvt_libs + algorithms_libs PRIVATE Boost::serialization Gflags::gflags diff --git a/src/core/libs/gnss_sdr_sample_counter.cc b/src/core/libs/gnss_sdr_sample_counter.cc index dc1412d41..612e5d3ff 100644 --- a/src/core/libs/gnss_sdr_sample_counter.cc +++ b/src/core/libs/gnss_sdr_sample_counter.cc @@ -17,6 +17,7 @@ #include "gnss_sdr_sample_counter.h" #include "gnss_synchro.h" +#include "gnss_time.h" #include #include // for from_double #include // for mp @@ -48,6 +49,7 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter( flag_m = false; flag_h = false; flag_days = false; + set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -57,6 +59,12 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int32_t _i return sample_counter_; } +int64_t gnss_sdr_sample_counter::uint64diff(uint64_t first, uint64_t second) +{ + uint64_t abs_diff = (first > second) ? (first - second) : (second - first); + assert(abs_diff <= INT64_MAX); + return (first > second) ? (int64_t)abs_diff : -(int64_t)abs_diff; +} int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), gr_vector_const_void_star &input_items __attribute__((unused)), @@ -129,5 +137,42 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), sample_counter += samples_per_output; out[0].Tracking_sample_counter = sample_counter; current_T_rx_ms += interval_ms; + + //**************** time tags **************** + std::vector tags_vec; + //notice that nitems_read is updated in decimation blocks after leaving work() with return 1, equivalent to call consume_each + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + samples_per_output); + for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + { + try + { + if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + //recompute timestamp to match the last sample in the consumed samples in this batch + int64_t diff_samplecount = uint64diff(out[0].Tracking_sample_counter, it->offset); + const std::shared_ptr last_timetag = boost::any_cast>(pmt::any_ref(it->value)); + double intpart; + last_timetag->tow_ms_fraction = modf(1000.0 * static_cast(diff_samplecount) / fs, &intpart); + + last_timetag->tow_ms = last_timetag->tow_ms + static_cast(intpart); + last_timetag->rx_time = static_cast(out[0].Tracking_sample_counter) / fs; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(last_timetag)); + //std::cout << "COUNTER TAG: this->nitems_read(0):" << this->nitems_read(0) << " sample_counter:" << sample_counter + // << " it->offset:" << it->offset << " diff:" << diff_samplecount << "\n"; + //getchar(); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + + //************* end time tags ************** + return 1; } diff --git a/src/core/libs/gnss_sdr_sample_counter.h b/src/core/libs/gnss_sdr_sample_counter.h index 40e54d47e..049be6345 100644 --- a/src/core/libs/gnss_sdr_sample_counter.h +++ b/src/core/libs/gnss_sdr_sample_counter.h @@ -58,6 +58,8 @@ private: int32_t _interval_ms, size_t _size); + int64_t uint64diff(uint64_t first, uint64_t second); + double fs; int64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run uint64_t sample_counter;