mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-02-07 14:40:12 +00:00
Code Issues Releases Wiki Activity

Add timetag clock offset configuration parameter in TImeTag File signal source. Timetag code clean

Browse Source
This commit is contained in:
Javier Arribas 2021-12-01 13:18:29 +01:00
parent c262d74e03
commit 8a4055b5b8
9 changed files with 36 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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(); const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s();
//**************** time tags **************** //**************** 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 ************* //************ 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; double delta_rxtime_to_tag_ms;
GnssTime current_tag; GnssTime current_tag;
//1. Find the nearest timetag to the current rx_time (it is relative to the receiver's start operation)
do do
{ {
current_tag = d_TimeChannelTagTimestamps.front(); 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()); 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] 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) if (d_log_timetag == true)
{ {
double current_corrected_RX_clock_ns = (d_rx_time - Rx_clock_offset_s) * 1e9; 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; 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); log_source_timetag_info(current_corrected_RX_clock_ns, TAG_time_ns);
double tow_error_ns = 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 std::cout << "[Time ch] RX TimeTag Week: " << current_tag.week
<< ", TOW: " << current_tag.tow_ms << ", TOW: " << current_tag.tow_ms
<< " [ms], TOW fraction: " << current_tag.tow_ms_fraction << " [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";
} }
} }
} }

58
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -622,47 +622,10 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
} }
} }
void hybrid_observables_gs::check_tag_timestamp(const std::vector<Gnss_Synchro> &data, uint64_t rx_clock) void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(const std::vector<Gnss_Synchro> &data, uint64_t rx_clock)
{ {
// std::vector<Gnss_Synchro>::const_iterator it; // it transforms the HW sample tag timestamp from a relative samplestamp (from receiver start)
// for (it = data.begin(); it != data.end(); it++) // to an absolute GPS TOW samplestamp associated with the current set of pseudoranges
// {
// if (!d_SourceTagTimestamps[it->Channel_ID].empty() and it->Flag_valid_pseudorange == true)
// {
// //std::cout << "RX Time: " << (static_cast<double>(rx_clock) / static_cast<double>(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<double>(rx_clock) / static_cast<double>(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<double>(current_tag.tow_ms) - static_cast<double>(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n";
//
// const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(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<int>(intpart);
// tmp_obj->rx_time = static_cast<double>(rx_clock) / static_cast<double>(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<double>(rx_clock) / static_cast<double>(it->fs)) << "s\n";
if (!d_TimeChannelTagTimestamps.empty()) if (!d_TimeChannelTagTimestamps.empty())
{ {
double fs = 0; double fs = 0;
@ -676,12 +639,12 @@ void hybrid_observables_gs::check_tag_timestamp(const std::vector<Gnss_Synchro>
} }
} }
double delta_rxtime_to_tag = 100; double delta_rxtime_to_tag;
GnssTime current_tag; GnssTime current_tag;
do do
{ {
current_tag = d_TimeChannelTagTimestamps.front(); current_tag = d_TimeChannelTagTimestamps.front();
delta_rxtime_to_tag = (static_cast<double>(rx_clock) / fs) - current_tag.rx_time; delta_rxtime_to_tag = (static_cast<double>(rx_clock) / fs) - current_tag.rx_time; //delta time relative to receiver's start time
if (delta_rxtime_to_tag >= 0) if (delta_rxtime_to_tag >= 0)
{ {
d_TimeChannelTagTimestamps.pop(); d_TimeChannelTagTimestamps.pop();
@ -697,24 +660,17 @@ void hybrid_observables_gs::check_tag_timestamp(const std::vector<Gnss_Synchro>
// << ", TOW: " << current_tag.tow_ms // << ", TOW: " << current_tag.tow_ms
// << " [ms], TOW fraction: " << current_tag.tow_ms_fraction // << " [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<double>(current_tag.tow_ms) - static_cast<double>(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n"; // << " [ms], DELTA TLM TOW: " << last_rx_clock_round20ms_error + delta_rxtime_to_tag * 1000.0 + static_cast<double>(current_tag.tow_ms) - static_cast<double>(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n";
const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime()); const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime());
*tmp_obj = current_tag; *tmp_obj = current_tag;
double intpart; double intpart;
tmp_obj->tow_ms_fraction = tmp_obj->tow_ms_fraction + modf(delta_rxtime_to_tag * 1000.0, &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<int>(intpart); tmp_obj->tow_ms = current_tag.tow_ms + static_cast<int>(intpart);
tmp_obj->rx_time = static_cast<double>(d_T_rx_TOW_ms); //static_cast<double>(rx_clock) / static_cast<double>(data.begin()->fs); tmp_obj->rx_time = static_cast<double>(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)); 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)), 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_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_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) if (n_valid > 0)
{ {
compute_pranges(epoch_data); 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) // Carrier smoothing (optional)

2
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
View File

@ -78,7 +78,7 @@ private:
void compute_pranges(std::vector<Gnss_Synchro>& data) const; void compute_pranges(std::vector<Gnss_Synchro>& data) const;
void smooth_pseudoranges(std::vector<Gnss_Synchro>& data); void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
void check_tag_timestamp(const std::vector<Gnss_Synchro>& data, uint64_t rx_clock); void set_tag_timestamp_in_sdr_timeframe(const std::vector<Gnss_Synchro>& data, uint64_t rx_clock);
int32_t save_matfile() const; int32_t save_matfile() const;
Obs_Conf d_conf; Obs_Conf d_conf;

7
src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc
View File

@ -24,7 +24,9 @@ using namespace std::string_literals;
FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration, FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration,
const std::string& role, unsigned int in_streams, unsigned int out_streams, const std::string& role, unsigned int in_streams, unsigned int out_streams,
Concurrent_Queue<pmt::pmt_t>* queue) Concurrent_Queue<pmt::pmt_t>* 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) if (in_streams > 0)
{ {
@ -50,7 +52,8 @@ void FileTimestampSignalSource::create_file_source_hook()
{ {
timestamp_block_ = gnss_sdr_make_Timestamp( timestamp_block_ = gnss_sdr_make_Timestamp(
std::get<0>(itemTypeToSize()), std::get<0>(itemTypeToSize()),
timestamp_file_); timestamp_file_,
timestamp_clock_offset_ms_);
DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")"; DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")";
} }

1
src/algorithms/signal_source/adapters/file_timestamp_signal_source.h
View File

@ -52,6 +52,7 @@ protected:
private: private:
gnss_shared_ptr<Gnss_Sdr_Timestamp> timestamp_block_; gnss_shared_ptr<Gnss_Sdr_Timestamp> timestamp_block_;
std::string timestamp_file_; std::string timestamp_file_;
double timestamp_clock_offset_ms_;
}; };

15
src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc
View File

@ -26,19 +26,20 @@
#include <cstring> // for memcpy #include <cstring> // for memcpy
Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item, Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item,
std::string timestamp_file) : gr::sync_block("Timestamp", 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),
gr::io_signature::make(1, 20, sizeof_stream_item)), gr::io_signature::make(1, 20, sizeof_stream_item)),
d_timefile(timestamp_file) d_timefile(timestamp_file),
d_clock_offset_ms(clock_offset_ms)
{ {
get_next_timetag = true; get_next_timetag = true;
next_timetag_samplecount = 0; next_timetag_samplecount = 0;
} }
gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp(size_t sizeof_stream_item, std::string timestamp_file) gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp(size_t sizeof_stream_item, std::string timestamp_file, double clock_offset_ms)
{ {
gnss_shared_ptr<Gnss_Sdr_Timestamp> Timestamp_(new Gnss_Sdr_Timestamp(sizeof_stream_item, timestamp_file)); gnss_shared_ptr<Gnss_Sdr_Timestamp> Timestamp_(new Gnss_Sdr_Timestamp(sizeof_stream_item, timestamp_file, clock_offset_ms));
return Timestamp_; return Timestamp_;
} }
@ -110,7 +111,7 @@ int Gnss_Sdr_Timestamp::work(int noutput_items,
const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime()); const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime());
tmp_obj->tow_ms = next_timetag.tow_ms; tmp_obj->tow_ms = next_timetag.tow_ms;
tmp_obj->week = next_timetag.week; 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; tmp_obj->rx_time = 0;
add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj)); 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";

10
src/algorithms/signal_source/libs/gnss_sdr_timestamp.h
View File

@ -38,7 +38,8 @@ class Gnss_Sdr_Timestamp;
gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp( gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp(
size_t sizeof_stream_item, 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 class Gnss_Sdr_Timestamp : public gr::sync_block
@ -52,16 +53,19 @@ public:
private: private:
friend gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp( friend gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp(
size_t sizeof_stream_item, 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, 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); int64_t uint64diff(uint64_t first, uint64_t second);
bool read_next_timetag(); bool read_next_timetag();
std::string d_timefile; std::string d_timefile;
std::fstream d_timefilestream; std::fstream d_timefilestream;
GnssTime next_timetag; GnssTime next_timetag;
double d_clock_offset_ms;
uint64_t next_timetag_samplecount; uint64_t next_timetag_samplecount;
bool get_next_timetag; bool get_next_timetag;
}; };

21
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@ -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_valid_symbol_output = !loss_of_lock;
current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked; current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked;
*out[0] = current_synchro_data; *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<double>(diff_samplecount) / d_trk_parameters.fs_in, &intpart);
//
// const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime());
// tmp_obj->week = d_last_timetag.week;
// tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast<int>(intpart);
// tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction;
// tmp_obj->rx_time = static_cast<double>(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<double>(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 1;
} }
return 0; return 0;

2
src/core/libs/gnss_sdr_sample_counter.cc
View File

@ -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); int64_t diff_samplecount = uint64diff(out[0].Tracking_sample_counter, it->offset);
const std::shared_ptr<GnssTime> last_timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value)); const std::shared_ptr<GnssTime> last_timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value));
double intpart; double intpart;
last_timetag->tow_ms_fraction = modf(1000.0 * static_cast<double>(diff_samplecount) / fs, &intpart); last_timetag->tow_ms_fraction += modf(1000.0 * static_cast<double>(diff_samplecount) / fs, &intpart);
last_timetag->tow_ms = last_timetag->tow_ms + static_cast<int>(intpart); last_timetag->tow_ms = last_timetag->tow_ms + static_cast<int>(intpart);
last_timetag->rx_time = static_cast<double>(out[0].Tracking_sample_counter) / fs; last_timetag->rx_time = static_cast<double>(out[0].Tracking_sample_counter) / fs;