Debug Build: Measuring signal timestamp vs. GNSS obs RX time

src/algorithms/libs/gnss_time.h

@@ -1,44 +1,26 @@
 /* -------------------------------------------------------------------------
  *
- * Copyright (C) 2019 (see AUTHORS file for a list of contributors)
+ * Copyright (C) 2021 (see AUTHORS file for a list of contributors)
  *
- * GNSS-SDR-SIM is a software defined Global Navigation
+ * GNSS-SDR is a software defined Global Navigation
  * Satellite Systems Simulator
  *
- * This file is part of GNSS-SDR-SIM.
+ * This file is part of GNSS-SDR.
  *
  */
 
-#ifndef GNSS_SDR_SIM_GNSS_TIME_H
+#ifndef GNSS_SDR_GNSS_TIME_H
-#define GNSS_SDR_SIM_GNSS_TIME_H
+#define GNSS_SDR_GNSS_TIME_H
 
-#include <boost/serialization/nvp.hpp>
 #include <cstdint>
-#include <time.h>
 
 class GnssTime
 {
 public:
-    //time_t time;         /* time (s) expressed by standard time_t */
+    double rx_time;
-    int week; /*!< GPS week number (since January 1980) */
+    int week;               /*!< GPS week number (since January 1980) */
-    //double sec;          /*!< second inside the GPS \a week */
     int tow_ms;             /* time of week [ms]*/
     double tow_ms_fraction; /* tow ms fractional part [ms]*/
-
-    template <class Archive>
-
-    /*!
-     * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
-     */
-    inline void serialize(Archive& archive, const uint32_t version)
-    {
-        using boost::serialization::make_nvp;
-        if (version)
-            {
-            };
-        archive& make_nvp("week", week);
-        archive& make_nvp("tow_ms", tow_ms);
-    };
 };
 
 #endif

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc

@@ -128,6 +128,8 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
     d_channel_last_pseudorange_smooth = std::vector<double>(d_nchannels_out, 0.0);
     d_channel_last_carrier_phase_rads = std::vector<double>(d_nchannels_out, 0.0);
 
+    d_SourceTagTimestamps = std::vector<std::queue<GnssTime>>(d_nchannels_out);
+
     d_smooth_filter_M = static_cast<double>(conf_.smoothing_factor);
     d_mapStringValues["1C"] = evGPS_1C;
     d_mapStringValues["2S"] = evGPS_2S;
@@ -601,6 +603,37 @@ 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)
+{
+    std::vector<Gnss_Synchro>::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<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.05 and !d_SourceTagTimestamps[it->Channel_ID].empty());
+
+                    if (fabs(delta_rxtime_to_tag) <= 0.05)
+                        {
+                            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: " << 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";
+                        }
+                }
+        }
+}
+
 
 int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)),
     gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
@@ -621,9 +654,34 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
     // 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++)
         {
-            // Push the valid tracking Gnss_Synchros to their corresponding deque
+            //**************** 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++)
                 {
+                    // Push the valid tracking Gnss_Synchros to their corresponding deque
                     if (in[n][m].Flag_valid_word)
                         {
                             if (d_gnss_synchro_history->size(n) > 0)
@@ -681,6 +739,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());
                 }
 
             // Carrier smoothing (optional)
@@ -688,6 +747,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
                 {
                     smooth_pseudoranges(epoch_data);
                 }
+
             // output the observables set to the PVT block
             for (uint32_t n = 0; n < d_nchannels_out; n++)
                 {

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h

@@ -21,6 +21,7 @@
 #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H
 
 #include "gnss_block_interface.h"
+#include "gnss_time.h"  //for timetags produced by Tracking
 #include "obs_conf.h"
 #include <boost/circular_buffer.hpp>  // for boost::circular_buffer
 #include <gnuradio/block.h>           // for block
@@ -30,9 +31,10 @@
 #include <fstream>                    // for std::ofstream
 #include <map>                        // for std::map
 #include <memory>                     // for std::shared, std:unique_ptr
-#include <string>                     // for std::string
+#include <queue>
-#include <typeinfo>                   // for typeid
+#include <string>    // for std::string
-#include <vector>                     // for std::vector
+#include <typeinfo>  // for typeid
+#include <vector>    // for std::vector
 
 /** \addtogroup Observables
  * \{ */
@@ -75,6 +77,8 @@ private:
     void update_TOW(const std::vector<Gnss_Synchro>& data);
     void compute_pranges(std::vector<Gnss_Synchro>& data) const;
     void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
+
+    void check_tag_timestamp(const std::vector<Gnss_Synchro>& data, uint64_t rx_clock);
     int32_t save_matfile() const;
 
     Obs_Conf d_conf;
@@ -101,6 +105,8 @@ private:
 
     boost::circular_buffer<uint64_t> d_Rx_clock_buffer;  // time history
 
+    std::vector<std::queue<GnssTime>> d_SourceTagTimestamps;
+
     std::vector<bool> d_channel_last_pll_lock;
     std::vector<double> d_channel_last_pseudorange_smooth;
     std::vector<double> d_channel_last_carrier_phase_rads;

src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc

@@ -110,8 +110,9 @@ int Gnss_Sdr_Timestamp::work(int noutput_items,
                     tmp_obj->tow_ms = next_timetag.tow_ms;
                     tmp_obj->week = next_timetag.week;
                     tmp_obj->tow_ms_fraction = 0;
+                    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";
+                    //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";
                     get_next_timetag = true;
                 }
         }

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc

@@ -532,8 +532,9 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__
                             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 << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction
+                                    //                                    std::cout << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction
-                                              << " [ms], DELTA TLM TOW: " << static_cast<double>(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n";
+                                    //                                              << " [ms], DELTA TLM TOW: " << static_cast<double>(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n";
+                                    add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(timetag));
                                 }
                             else
                                 {

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -2047,10 +2047,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
                     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 << "] 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 << "] 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;
                 }