diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index bccb5b0e1..745a9ebc4 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -818,6 +818,11 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // Set maximum clock offset allowed if pvt_output_parameters.enable_rx_clock_correction = false pvt_output_parameters.max_obs_block_rx_clock_offset_ms = configuration->property(role + ".max_clock_offset_ms", pvt_output_parameters.max_obs_block_rx_clock_offset_ms); + + //Source timetag + pvt_output_parameters.log_source_timetag = configuration->property(role + ".log_timetag", pvt_output_parameters.log_source_timetag); + pvt_output_parameters.log_source_timetag_file = configuration->property(role + ".log_source_timetag_file", pvt_output_parameters.log_source_timetag_file); + // make PVT object pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index f7714a6be..452b572a2 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -513,6 +513,13 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_beidou_dnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_beidou_dnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); + //timetag + d_log_timetag = conf_.log_source_timetag; + if (d_log_timetag) + { + d_log_timetag_file = std::fstream(conf_.log_source_timetag_file, std::ios::out | std::ios::binary); + std::cout << "Log PVT timetag metadata enabled, log file: " << conf_.log_source_timetag_file << "\n"; + } d_start = std::chrono::system_clock::now(); } @@ -1060,6 +1067,11 @@ rtklib_pvt_gs::~rtklib_pvt_gs() LOG(INFO) << "Failed to save BeiDou DNAV UTC model parameters, not valid data"; } } + + if (d_log_timetag_file.is_open()) + { + d_log_timetag_file.close(); + } } catch (std::length_error& e) { @@ -1573,6 +1585,14 @@ bool rtklib_pvt_gs::save_gnss_synchro_map_xml(const std::string& file_name) return false; } +void rtklib_pvt_gs::log_source_timetag_info(double RX_time_ns, double TAG_time_ns) +{ + if (d_log_timetag_file.is_open()) + { + d_log_timetag_file.write(reinterpret_cast(&RX_time_ns), sizeof(double)); + d_log_timetag_file.write(reinterpret_cast(&TAG_time_ns), sizeof(double)); + } +} bool rtklib_pvt_gs::load_gnss_synchro_map_xml(const std::string& file_name) { @@ -1979,6 +1999,45 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false)) { 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 + { + //************ Source TimeTag comparison with GNSS computed TOW ************* + + if (!d_TimeChannelTagTimestamps.empty()) + { + double delta_rxtime_to_tag_ms; + GnssTime current_tag; + do + { + current_tag = d_TimeChannelTagTimestamps.front(); + delta_rxtime_to_tag_ms = d_rx_time * 1000.0 - current_tag.rx_time; + d_TimeChannelTagTimestamps.pop(); + } + while (fabs(delta_rxtime_to_tag_ms) >= 100 and !d_TimeChannelTagTimestamps.empty()); + + + if (fabs(delta_rxtime_to_tag_ms) <= 100) //[ms] + { + 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; + 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 (fabs(Rx_clock_offset_s) * 1000.0 > d_max_obs_block_rx_clock_offset_ms) { if (!d_waiting_obs_block_rx_clock_offset_correction_msg) @@ -2062,36 +2121,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item 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 c3f95148e..370a7b4fe 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -133,6 +133,8 @@ private: const Pvt_Conf& conf_, const rtk_t& rtk); + void log_source_timetag_info(double RX_time_ns, double TAG_time_ns); + void msg_handler_telemetry(const pmt::pmt_t& msg); void initialize_and_apply_carrier_phase_offset(); @@ -161,6 +163,9 @@ private: bool save_gnss_synchro_map_xml(const std::string& file_name); // debug helper function bool load_gnss_synchro_map_xml(const std::string& file_name); // debug helper function + bool d_log_timetag; + std::fstream d_log_timetag_file; + std::shared_ptr d_internal_pvt_solver; std::shared_ptr d_user_pvt_solver; diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc index d5951940c..db4c12917 100644 --- a/src/algorithms/PVT/libs/pvt_conf.cc +++ b/src/algorithms/PVT/libs/pvt_conf.cc @@ -67,4 +67,7 @@ Pvt_Conf::Pvt_Conf() udp_eph_port = 0; pre_2009_file = false; show_local_time_zone = false; + + log_source_timetag = false; + log_source_timetag_file = "PVT_timetag.dat"; } diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 147e5109b..6ff70abcc 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -85,6 +85,9 @@ public: bool pre_2009_file; bool dump; bool dump_mat; + + bool log_source_timetag; + std::string log_source_timetag_file; };