mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Adding PVT timetag log recording option

Browse Source
This commit is contained in:
Javier Arribas 2021-04-29 16:14:35 +02:00
parent 2fe2087310
commit 1e9315900c
5 changed files with 75 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -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() << ")";

89
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -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<Beidou_Dnav_Utc_Model>).hash_code();
d_beidou_dnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr<Beidou_Dnav_Almanac>).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<char*>(&RX_time_ns), sizeof(double));
d_log_timetag_file.write(reinterpret_cast<char*>(&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<double>(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<double>(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<double>(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]"

5
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@ -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<Rtklib_Solver> d_internal_pvt_solver;
std::shared_ptr<Rtklib_Solver> d_user_pvt_solver;

3
src/algorithms/PVT/libs/pvt_conf.cc
View File

@ -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";
}

3
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -85,6 +85,9 @@ public:
bool pre_2009_file;
bool dump;
bool dump_mat;
bool log_source_timetag;
std::string log_source_timetag_file;
};