From 26bc9c9b5a0e86a7f577be27e794077dca7efc20 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 12 Apr 2021 18:37:06 +0200 Subject: [PATCH] Debug Build: Measuring signal timestamp vs. GNSS obs RX time --- src/algorithms/libs/gnss_time.h | 32 +++------- .../gnuradio_blocks/hybrid_observables_gs.cc | 62 ++++++++++++++++++- .../gnuradio_blocks/hybrid_observables_gs.h | 12 +++- .../signal_source/libs/gnss_sdr_timestamp.cc | 3 +- .../gps_l1_ca_telemetry_decoder_gs.cc | 5 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +- 6 files changed, 84 insertions(+), 34 deletions(-) diff --git a/src/algorithms/libs/gnss_time.h b/src/algorithms/libs/gnss_time.h index 391f61f4e..60fc7f06b 100644 --- a/src/algorithms/libs/gnss_time.h +++ b/src/algorithms/libs/gnss_time.h @@ -1,44 +1,26 @@ /* ------------------------------------------------------------------------- * - * Copyright (C) 2019 (see AUTHORS file for a list of contributors) + * Copyright (C) 2021 (see AUTHORS file for a list of contributors) * - * GNSS-SDR-SIM is a software defined Global Navigation + * GNSS-SDR is a software defined Global Navigation * Satellite Systems Simulator * - * This file is part of GNSS-SDR-SIM. + * This file is part of GNSS-SDR. * */ -#ifndef GNSS_SDR_SIM_GNSS_TIME_H -#define GNSS_SDR_SIM_GNSS_TIME_H +#ifndef GNSS_SDR_GNSS_TIME_H +#define GNSS_SDR_GNSS_TIME_H -#include #include -#include class GnssTime { public: - //time_t time; /* time (s) expressed by standard time_t */ - int week; /*!< GPS week number (since January 1980) */ - //double sec; /*!< second inside the GPS \a week */ + double rx_time; + int week; /*!< GPS week number (since January 1980) */ int tow_ms; /* time of week [ms]*/ double tow_ms_fraction; /* tow ms fractional part [ms]*/ - - template - - /*! - * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. - */ - inline void serialize(Archive& archive, const uint32_t version) - { - using boost::serialization::make_nvp; - if (version) - { - }; - archive& make_nvp("week", week); - archive& make_nvp("tow_ms", tow_ms); - }; }; #endif diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index c9aae2632..72f8389c6 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -128,6 +128,8 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block( d_channel_last_pseudorange_smooth = std::vector(d_nchannels_out, 0.0); d_channel_last_carrier_phase_rads = std::vector(d_nchannels_out, 0.0); + d_SourceTagTimestamps = std::vector>(d_nchannels_out); + d_smooth_filter_M = static_cast(conf_.smoothing_factor); d_mapStringValues["1C"] = evGPS_1C; d_mapStringValues["2S"] = evGPS_2S; @@ -601,6 +603,37 @@ 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()); + + if (fabs(delta_rxtime_to_tag) <= 0.05) + { + 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"; + } + } + } +} + int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, @@ -621,9 +654,34 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) // 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++) { - // Push the valid tracking Gnss_Synchros to their corresponding deque + //**************** 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 if (in[n][m].Flag_valid_word) { if (d_gnss_synchro_history->size(n) > 0) @@ -681,6 +739,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) if (n_valid > 0) { compute_pranges(epoch_data); + check_tag_timestamp(epoch_data, d_Rx_clock_buffer.front()); } // Carrier smoothing (optional) @@ -688,6 +747,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { smooth_pseudoranges(epoch_data); } + // output the observables set to the PVT block for (uint32_t n = 0; n < d_nchannels_out; n++) { diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 2f3551cd3..0530f7c87 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -21,6 +21,7 @@ #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H #include "gnss_block_interface.h" +#include "gnss_time.h" //for timetags produced by Tracking #include "obs_conf.h" #include // for boost::circular_buffer #include // for block @@ -30,9 +31,10 @@ #include // for std::ofstream #include // for std::map #include // for std::shared, std:unique_ptr -#include // for std::string -#include // for typeid -#include // for std::vector +#include +#include // for std::string +#include // for typeid +#include // for std::vector /** \addtogroup Observables * \{ */ @@ -75,6 +77,8 @@ private: void update_TOW(const std::vector& data); void compute_pranges(std::vector& data) const; void smooth_pseudoranges(std::vector& data); + + void check_tag_timestamp(const std::vector& data, uint64_t rx_clock); int32_t save_matfile() const; Obs_Conf d_conf; @@ -101,6 +105,8 @@ private: boost::circular_buffer d_Rx_clock_buffer; // time history + std::vector> d_SourceTagTimestamps; + std::vector d_channel_last_pll_lock; std::vector d_channel_last_pseudorange_smooth; std::vector d_channel_last_carrier_phase_rads; diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc index 42f0b0070..36a350396 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc @@ -110,8 +110,9 @@ int Gnss_Sdr_Timestamp::work(int noutput_items, tmp_obj->tow_ms = next_timetag.tow_ms; tmp_obj->week = next_timetag.week; tmp_obj->tow_ms_fraction = 0; + tmp_obj->rx_time = 0; add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj)); - std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; + //std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; get_next_timetag = true; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index d3999ef29..67222185c 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -532,8 +532,9 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ 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 << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction - << " [ms], DELTA TLM TOW: " << static_cast(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n"; + // std::cout << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction + // << " [ms], DELTA TLM TOW: " << static_cast(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n"; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(timetag)); } else { 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 fcdadf2f2..fd4d7776f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -2047,10 +2047,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) 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"; + //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; }