mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Move External TimeTag propagation to the time counter channel. PVT OBS timestamp comarison completed

This commit is contained in:
Javier Arribas 2021-04-19 12:27:02 +02:00
parent 26bc9c9b5a
commit 5c3134dbde
8 changed files with 266 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1816,6 +1816,35 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset()
int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items __attribute__((unused))) gr_vector_void_star& output_items __attribute__((unused)))
{ {
//**************** time tags ****************
if (d_enable_rx_clock_correction == false) //todo: currently only works if clock correction is disabled
{
std::vector<gr::tag_t> tags_vec;
//time tag from obs to pvt is always propagated in channel 0
this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items);
for (std::vector<gr::tag_t>::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<GnssTime>).hash_code())
{
const std::shared_ptr<GnssTime> timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value));
//std::cout << "PVT timetag: " << timetag->rx_time << "\n";
d_TimeChannelTagTimestamps.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 epoch = 0; epoch < noutput_items; epoch++) for (int32_t epoch = 0; epoch < noutput_items; epoch++)
{ {
bool flag_display_pvt = false; bool flag_display_pvt = false;
@ -1999,7 +2028,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
{ {
flag_compute_pvt_output = true; flag_compute_pvt_output = true;
// std::cout.precision(17); // std::cout.precision(17);
// std::cout << "current_RX_time: " << current_RX_time << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n'; //std::cout << "current_RX_time: " << current_RX_time_ms << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n';
} }
flag_pvt_valid = true; flag_pvt_valid = true;
} }
@ -2017,20 +2046,52 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false); flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false);
} }
if (flag_pvt_valid == true) if (flag_pvt_valid == true)
{ {
//experimental VTL tests //experimental VTL tests
// send tracking command // send tracking command
const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd()); // const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
trk_cmd_test->carrier_freq_hz = 12345.4; // trk_cmd_test->carrier_freq_hz = 12345.4;
trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter; // trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter;
this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// initialize (if needed) the accumulated phase offset and apply it to the active channels // initialize (if needed) the accumulated phase offset and apply it to the active channels
// required to report accumulated phase cycles comparable to pseudoranges // required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset(); initialize_and_apply_carrier_phase_offset();
const double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s(); 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 !! 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]" LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]"

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

@ -19,6 +19,7 @@
#include "gnss_block_interface.h" #include "gnss_block_interface.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "gnss_time.h"
#include "rtklib.h" #include "rtklib.h"
#include <boost/date_time/gregorian/gregorian.hpp> #include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
@ -31,9 +32,10 @@
#include <ctime> // for time_t #include <ctime> // for time_t
#include <map> // for map #include <map> // for map
#include <memory> // for shared_ptr, unique_ptr #include <memory> // for shared_ptr, unique_ptr
#include <string> // for string #include <queue>
#include <sys/types.h> // for key_t #include <string> // for string
#include <vector> // for vector #include <sys/types.h> // for key_t
#include <vector> // for vector
/** \addtogroup PVT /** \addtogroup PVT
* \{ */ * \{ */
@ -202,6 +204,8 @@ private:
std::map<int, Gnss_Synchro> d_gnss_observables_map_t0; std::map<int, Gnss_Synchro> d_gnss_observables_map_t0;
std::map<int, Gnss_Synchro> d_gnss_observables_map_t1; std::map<int, Gnss_Synchro> d_gnss_observables_map_t1;
std::queue<GnssTime> d_TimeChannelTagTimestamps;
boost::posix_time::time_duration d_utc_diff_time; boost::posix_time::time_duration d_utc_diff_time;
size_t d_gps_ephemeris_sptr_type_hash_code; size_t d_gps_ephemeris_sptr_type_hash_code;

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

@ -143,6 +143,8 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
d_mapStringValues["B1"] = evBDS_B1; d_mapStringValues["B1"] = evBDS_B1;
d_mapStringValues["B2"] = evBDS_B2; d_mapStringValues["B2"] = evBDS_B2;
d_mapStringValues["B3"] = evBDS_B3; d_mapStringValues["B3"] = evBDS_B3;
last_rx_clock_round20ms_error = 0;
set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work()
} }
@ -192,12 +194,16 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
if (pmt::any_ref(msg).type().hash_code() == d_double_type_hash_code) if (pmt::any_ref(msg).type().hash_code() == d_double_type_hash_code)
{ {
const auto new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg)); const auto new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
double old_tow_corrected = static_cast<double>(d_T_rx_TOW_ms) - new_rx_clock_offset_s * 1000.0;
d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0)); d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
// align the receiver clock to integer multiple of 20 ms // align the receiver clock to integer multiple of 20 ms
if (d_T_rx_TOW_ms % 20) if (d_T_rx_TOW_ms % 20)
{ {
d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20; d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20;
} }
last_rx_clock_round20ms_error = static_cast<double>(d_T_rx_TOW_ms) - old_tow_corrected;
// d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer // d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {
@ -605,32 +611,93 @@ 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::check_tag_timestamp(const std::vector<Gnss_Synchro> &data, uint64_t rx_clock)
{ {
std::vector<Gnss_Synchro>::const_iterator it; // std::vector<Gnss_Synchro>::const_iterator it;
for (it = data.begin(); it != data.end(); it++) // for (it = data.begin(); it != data.end(); it++)
{ // {
if (!d_SourceTagTimestamps[it->Channel_ID].empty() and it->Flag_valid_pseudorange == true) // 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"; // //std::cout << "RX Time: " << (static_cast<double>(rx_clock) / static_cast<double>(it->fs)) << "s\n";
double delta_rxtime_to_tag; // double delta_rxtime_to_tag;
GnssTime current_tag; // GnssTime current_tag;
do // do
{ // {
current_tag = d_SourceTagTimestamps[it->Channel_ID].front(); // 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; // 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"; // // std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag << "]\n";
d_SourceTagTimestamps[it->Channel_ID].pop(); // d_SourceTagTimestamps[it->Channel_ID].pop();
} // }
while (fabs(delta_rxtime_to_tag) >= 0.05 and !d_SourceTagTimestamps[it->Channel_ID].empty()); // 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));
// }
// }
// }
if (fabs(delta_rxtime_to_tag) <= 0.05) //std::cout << "RX Time: " << (static_cast<double>(rx_clock) / static_cast<double>(it->fs)) << "s\n";
if (!d_TimeChannelTagTimestamps.empty())
{
double fs = 0;
std::vector<Gnss_Synchro>::const_iterator it;
for (it = data.begin(); it != data.end(); it++)
{
if (it->Flag_valid_pseudorange == true)
{ {
std::cout << "[ch:" << it->Channel_ID << "][" << delta_rxtime_to_tag fs = static_cast<double>(it->fs);
<< "] OBS RX TimeTag Week: " << current_tag.week break;
<< ", 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<double>(current_tag.tow_ms) - it->RX_time * 1000.0 + current_tag.tow_ms_fraction << " [ms] \n";
} }
} }
double delta_rxtime_to_tag = 100;
GnssTime current_tag;
do
{
current_tag = d_TimeChannelTagTimestamps.front();
delta_rxtime_to_tag = (static_cast<double>(rx_clock) / fs) - current_tag.rx_time;
if (delta_rxtime_to_tag >= 0)
{
d_TimeChannelTagTimestamps.pop();
}
}
while (delta_rxtime_to_tag >= 0.1 and !d_TimeChannelTagTimestamps.empty());
if (delta_rxtime_to_tag >= 0 and delta_rxtime_to_tag <= 0.1)
{
// std::cout << "[Time ch][" << 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;
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<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);
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";
// }
} }
} }
@ -647,16 +714,10 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
if (ninput_items[d_nchannels_in - 1] > 0) if (ninput_items[d_nchannels_in - 1] > 0)
{ {
d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter); d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter);
// Consume one item from the clock channel (last of the input channels)
consume(static_cast<int32_t>(d_nchannels_in) - 1, 1);
}
// 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++)
{
//**************** time tags **************** //**************** time tags ****************
std::vector<gr::tag_t> tags_vec; std::vector<gr::tag_t> tags_vec;
this->get_tags_in_range(tags_vec, n, this->nitems_read(n), this->nitems_read(n) + ninput_items[n]); this->get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1);
for (std::vector<gr::tag_t>::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) for (std::vector<gr::tag_t>::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it)
{ {
try try
@ -664,8 +725,8 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr<GnssTime>).hash_code()) if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr<GnssTime>).hash_code())
{ {
const std::shared_ptr<GnssTime> timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value)); const std::shared_ptr<GnssTime> timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value));
//std::cout << "[ch " << n << "] timetag: " << timetag->rx_time << "\n"; //std::cout << "[Time ch ] timetag: " << timetag->rx_time << "\n";
d_SourceTagTimestamps.at(n).push(*timetag); d_TimeChannelTagTimestamps.push(*timetag);
} }
else else
{ {
@ -679,6 +740,40 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
} }
//************* end time tags ************** //************* end time tags **************
// Consume one item from the clock channel (last of the input channels)
consume(static_cast<int32_t>(d_nchannels_in) - 1, 1);
}
// 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++)
{
//**************** time tags ****************
// std::vector<gr::tag_t> tags_vec;
// this->get_tags_in_range(tags_vec, n, this->nitems_read(n), this->nitems_read(n) + ninput_items[n]);
// for (std::vector<gr::tag_t>::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<GnssTime>).hash_code())
// {
// const std::shared_ptr<GnssTime> timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(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++) for (int32_t m = 0; m < ninput_items[n]; m++)
{ {
// Push the valid tracking Gnss_Synchros to their corresponding deque // Push the valid tracking Gnss_Synchros to their corresponding deque

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

@ -106,6 +106,7 @@ private:
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
std::vector<std::queue<GnssTime>> d_SourceTagTimestamps; std::vector<std::queue<GnssTime>> d_SourceTagTimestamps;
std::queue<GnssTime> d_TimeChannelTagTimestamps;
std::vector<bool> d_channel_last_pll_lock; std::vector<bool> d_channel_last_pll_lock;
std::vector<double> d_channel_last_pseudorange_smooth; std::vector<double> d_channel_last_pseudorange_smooth;
@ -118,6 +119,7 @@ private:
double d_smooth_filter_M; double d_smooth_filter_M;
uint32_t d_T_rx_TOW_ms; uint32_t d_T_rx_TOW_ms;
double last_rx_clock_round20ms_error;
uint32_t d_T_rx_step_ms; uint32_t d_T_rx_step_ms;
uint32_t d_T_status_report_timer_ms; uint32_t d_T_status_report_timer_ms;
uint32_t d_nchannels_in; uint32_t d_nchannels_in;

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

@ -2036,23 +2036,23 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
//generate new tag associated with gnss-synchro object //generate new tag associated with gnss-synchro object
if (d_timetag_waiting == true) // if (d_timetag_waiting == true)
{ // {
int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); // int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter);
//
double intpart; // double intpart;
d_last_timetag.tow_ms_fraction = modf(1000.0 * static_cast<double>(diff_samplecount) / d_trk_parameters.fs_in, &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()); // const std::shared_ptr<GnssTime> tmp_obj = std::make_shared<GnssTime>(GnssTime());
tmp_obj->week = d_last_timetag.week; // tmp_obj->week = d_last_timetag.week;
tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast<int>(intpart); // 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->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; // 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)); // 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 << "][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; // d_timetag_waiting = false;
} // }
return 1; return 1;
} }

1
src/core/libs/CMakeLists.txt
View File

@ -76,6 +76,7 @@ target_link_libraries(core_libs
core_libs_supl core_libs_supl
core_system_parameters core_system_parameters
pvt_libs pvt_libs
algorithms_libs
PRIVATE PRIVATE
Boost::serialization Boost::serialization
Gflags::gflags Gflags::gflags

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

@ -17,6 +17,7 @@
#include "gnss_sdr_sample_counter.h" #include "gnss_sdr_sample_counter.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "gnss_time.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <pmt/pmt.h> // for from_double #include <pmt/pmt.h> // for from_double
#include <pmt/pmt_sugar.h> // for mp #include <pmt/pmt_sugar.h> // for mp
@ -48,6 +49,7 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(
flag_m = false; flag_m = false;
flag_h = false; flag_h = false;
flag_days = false; flag_days = false;
set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work()
} }
@ -57,6 +59,12 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int32_t _i
return sample_counter_; return sample_counter_;
} }
int64_t gnss_sdr_sample_counter::uint64diff(uint64_t first, uint64_t second)
{
uint64_t abs_diff = (first > second) ? (first - second) : (second - first);
assert(abs_diff <= INT64_MAX);
return (first > second) ? (int64_t)abs_diff : -(int64_t)abs_diff;
}
int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_const_void_star &input_items __attribute__((unused)),
@ -129,5 +137,42 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
sample_counter += samples_per_output; sample_counter += samples_per_output;
out[0].Tracking_sample_counter = sample_counter; out[0].Tracking_sample_counter = sample_counter;
current_T_rx_ms += interval_ms; current_T_rx_ms += interval_ms;
//**************** time tags ****************
std::vector<gr::tag_t> tags_vec;
//notice that nitems_read is updated in decimation blocks after leaving work() with return 1, equivalent to call consume_each
this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + samples_per_output);
for (std::vector<gr::tag_t>::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<GnssTime>).hash_code())
{
//recompute timestamp to match the last sample in the consumed samples in this batch
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));
double 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->rx_time = static_cast<double>(out[0].Tracking_sample_counter) / fs;
add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(last_timetag));
//std::cout << "COUNTER TAG: this->nitems_read(0):" << this->nitems_read(0) << " sample_counter:" << sample_counter
// << " it->offset:" << it->offset << " diff:" << diff_samplecount << "\n";
//getchar();
}
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 **************
return 1; return 1;
} }

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

@ -58,6 +58,8 @@ private:
int32_t _interval_ms, int32_t _interval_ms,
size_t _size); size_t _size);
int64_t uint64diff(uint64_t first, uint64_t second);
double fs; double fs;
int64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run int64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run
uint64_t sample_counter; uint64_t sample_counter;