diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 442d19279..b4084cd35 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2090,7 +2090,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); //**************** time tags **************** - if (d_enable_rx_clock_correction == false) //todo: currently only works if clock correction is disabled + if (d_enable_rx_clock_correction == false) //todo: currently only works if clock correction is disabled (computed clock offset is applied here) { //************ Source TimeTag comparison with GNSS computed TOW ************* @@ -2098,6 +2098,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { double delta_rxtime_to_tag_ms; GnssTime current_tag; + //1. Find the nearest timetag to the current rx_time (it is relative to the receiver's start operation) do { current_tag = d_TimeChannelTagTimestamps.front(); @@ -2107,20 +2108,20 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item while (fabs(delta_rxtime_to_tag_ms) >= 100 and !d_TimeChannelTagTimestamps.empty()); + //2. If both timestamps (relative to the receiver's start) are closer than 100 ms (the granularituy of the PVT) if (fabs(delta_rxtime_to_tag_ms) <= 100) //[ms] { + std::cout << "GNSS-SDR RX TIME: " << d_rx_time << " TAG RX TIME: " << current_tag.rx_time / 1000.0 << " [s]\n"; if (d_log_timetag == true) { double current_corrected_RX_clock_ns = (d_rx_time - Rx_clock_offset_s) * 1e9; double TAG_time_ns = (static_cast(current_tag.tow_ms) + current_tag.tow_ms_fraction + delta_rxtime_to_tag_ms) * 1e6; log_source_timetag_info(current_corrected_RX_clock_ns, TAG_time_ns); - - // 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); - double timestamp_tow_error_ns = TAG_time_ns - current_corrected_RX_clock_ns; + double tow_error_ns = current_corrected_RX_clock_ns - TAG_time_ns; 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"; + << " [ms], GNSS-SDR OBS CORRECTED TOW - EXTERNAL TIMETAG TOW: " << tow_error_ns << " [ns] \n"; } } } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index 8b887af51..f9d950af9 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -622,47 +622,10 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector &data) } } -void hybrid_observables_gs::check_tag_timestamp(const std::vector &data, uint64_t rx_clock) +void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(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.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)); - // } - // } - // } - - //std::cout << "RX Time: " << (static_cast(rx_clock) / static_cast(it->fs)) << "s\n"; - + // it transforms the HW sample tag timestamp from a relative samplestamp (from receiver start) + // to an absolute GPS TOW samplestamp associated with the current set of pseudoranges if (!d_TimeChannelTagTimestamps.empty()) { double fs = 0; @@ -676,12 +639,12 @@ void hybrid_observables_gs::check_tag_timestamp(const std::vector } } - double delta_rxtime_to_tag = 100; + double delta_rxtime_to_tag; GnssTime current_tag; do { current_tag = d_TimeChannelTagTimestamps.front(); - delta_rxtime_to_tag = (static_cast(rx_clock) / fs) - current_tag.rx_time; + delta_rxtime_to_tag = (static_cast(rx_clock) / fs) - current_tag.rx_time; //delta time relative to receiver's start time if (delta_rxtime_to_tag >= 0) { d_TimeChannelTagTimestamps.pop(); @@ -697,24 +660,17 @@ void hybrid_observables_gs::check_tag_timestamp(const std::vector // << ", 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); + tmp_obj->rx_time = static_cast(d_T_rx_TOW_ms); // new TAG samplestamp in absolute RX time (GPS TOW frame) same as the pseudorange set 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"; - // } } } - int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -847,7 +803,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()); + set_tag_timestamp_in_sdr_timeframe(epoch_data, d_Rx_clock_buffer.front()); } // Carrier smoothing (optional) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 909c26a21..e19047082 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -78,7 +78,7 @@ private: 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); + void set_tag_timestamp_in_sdr_timeframe(const std::vector& data, uint64_t rx_clock); int32_t save_matfile() const; Obs_Conf d_conf; diff --git a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc index 7189024b7..dfe9249f9 100644 --- a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc +++ b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc @@ -24,7 +24,9 @@ using namespace std::string_literals; FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams, Concurrent_Queue* queue) - : FileSourceBase(configuration, role, "File_Timestamp_Signal_Source"s, queue, "byte"s), timestamp_file_(configuration->property(role + ".timestamp_filename"s, "../data/example_capture_timestamp.dat"s)) + : FileSourceBase(configuration, role, "File_Timestamp_Signal_Source"s, queue, "byte"s), + timestamp_file_(configuration->property(role + ".timestamp_filename"s, "../data/example_capture_timestamp.dat"s)), + timestamp_clock_offset_ms_(configuration->property(role + ".timestamp_clock_offset_ms"s, 0.0)) { if (in_streams > 0) { @@ -50,7 +52,8 @@ void FileTimestampSignalSource::create_file_source_hook() { timestamp_block_ = gnss_sdr_make_Timestamp( std::get<0>(itemTypeToSize()), - timestamp_file_); + timestamp_file_, + timestamp_clock_offset_ms_); DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")"; } diff --git a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h index 7864d1555..75a8d2450 100644 --- a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h +++ b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h @@ -52,6 +52,7 @@ protected: private: gnss_shared_ptr timestamp_block_; std::string timestamp_file_; + double timestamp_clock_offset_ms_; }; diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc index fa910ac74..c0a385fa0 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc @@ -26,19 +26,20 @@ #include // for memcpy Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item, - std::string timestamp_file) : gr::sync_block("Timestamp", - gr::io_signature::make(1, 20, sizeof_stream_item), - gr::io_signature::make(1, 20, sizeof_stream_item)), - d_timefile(timestamp_file) + std::string timestamp_file, double clock_offset_ms) : gr::sync_block("Timestamp", + gr::io_signature::make(1, 20, sizeof_stream_item), + gr::io_signature::make(1, 20, sizeof_stream_item)), + d_timefile(timestamp_file), + d_clock_offset_ms(clock_offset_ms) { get_next_timetag = true; next_timetag_samplecount = 0; } -gnss_shared_ptr gnss_sdr_make_Timestamp(size_t sizeof_stream_item, std::string timestamp_file) +gnss_shared_ptr gnss_sdr_make_Timestamp(size_t sizeof_stream_item, std::string timestamp_file, double clock_offset_ms) { - gnss_shared_ptr Timestamp_(new Gnss_Sdr_Timestamp(sizeof_stream_item, timestamp_file)); + gnss_shared_ptr Timestamp_(new Gnss_Sdr_Timestamp(sizeof_stream_item, timestamp_file, clock_offset_ms)); return Timestamp_; } @@ -110,7 +111,7 @@ int Gnss_Sdr_Timestamp::work(int noutput_items, const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); tmp_obj->tow_ms = next_timetag.tow_ms; tmp_obj->week = next_timetag.week; - tmp_obj->tow_ms_fraction = 0; + tmp_obj->tow_ms_fraction = d_clock_offset_ms; //optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration 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"; diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h index cf7bd2d92..6904a7c0d 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h @@ -38,7 +38,8 @@ class Gnss_Sdr_Timestamp; gnss_shared_ptr gnss_sdr_make_Timestamp( size_t sizeof_stream_item, - std::string timestamp_file); + std::string timestamp_file, + double clock_offset_ms); class Gnss_Sdr_Timestamp : public gr::sync_block @@ -52,16 +53,19 @@ public: private: friend gnss_shared_ptr gnss_sdr_make_Timestamp( size_t sizeof_stream_item, - std::string timestamp_file); + std::string timestamp_file, + double clock_offset_ms); Gnss_Sdr_Timestamp(size_t sizeof_stream_item, - std::string timestamp_file); + std::string timestamp_file, + double clock_offset_ms); int64_t uint64diff(uint64_t first, uint64_t second); bool read_next_timetag(); std::string d_timefile; std::fstream d_timefilestream; GnssTime next_timetag; + double d_clock_offset_ms; uint64_t next_timetag_samplecount; bool get_next_timetag; }; 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 3cbf53cd9..c65edff8d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -2055,27 +2055,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = !loss_of_lock; current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked; *out[0] = current_synchro_data; - - //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 << "][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; } return 0; diff --git a/src/core/libs/gnss_sdr_sample_counter.cc b/src/core/libs/gnss_sdr_sample_counter.cc index b27eef995..a0040a128 100644 --- a/src/core/libs/gnss_sdr_sample_counter.cc +++ b/src/core/libs/gnss_sdr_sample_counter.cc @@ -152,7 +152,7 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), 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_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;