Adding timetag propagation from source to telemetry decoders (currently supported by Galileo telemetry decoder)

src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc

@@ -32,6 +32,7 @@ Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item,
                                                           d_timefile(timestamp_file),
                                                           d_clock_offset_ms(clock_offset_ms)
 {
+    d_fraction_ms_offset = modf(d_clock_offset_ms, &d_integer_ms_offset);  // optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration
     get_next_timetag = true;
     next_timetag_samplecount = 0;
 }
@@ -95,26 +96,26 @@ int Gnss_Sdr_Timestamp::work(int noutput_items,
         {
             if (read_next_timetag() == false)
                 {
-                    //std::cout << "End of TimeTag file reached!\n";
+                    // std::cout << "End of TimeTag file reached!\n";
-                    //return 0;  //todo: find why return -1 does not stop gnss-sdr!
+                    // return 0;  // todo: find why return -1 does not stop gnss-sdr!
                 }
             get_next_timetag = false;
         }
     for (size_t ch = 0; ch < output_items.size(); ch++)
         {
             std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch));
-            uint64_t bytes_to_samples = 2;  //todo: improve this.. hardcoded 2 bytes -> 1 complex sample!
+            uint64_t bytes_to_samples = 2;  // todo: improve this.. hardcoded 2 bytes -> 1 complex sample!
             int64_t diff_samplecount = uint64diff(this->nitems_written(ch), next_timetag_samplecount * bytes_to_samples);
-            //std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n";
+            // std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n";
             if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items)
                 {
                     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 + static_cast<int>(d_integer_ms_offset);
                     tmp_obj->week = next_timetag.week;
-                    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->tow_ms_fraction = d_fraction_ms_offset;
                     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/signal_source/libs/gnss_sdr_timestamp.h

@@ -66,6 +66,8 @@ private:
     std::fstream d_timefilestream;
     GnssTime next_timetag;
     double d_clock_offset_ms;
+    double d_integer_ms_offset;
+    double d_fraction_ms_offset;
     uint64_t next_timetag_samplecount;
     bool get_next_timetag;
 };

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc

@@ -685,6 +685,51 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
             }
         }
     d_sample_counter++;  // count for the processed symbols
+
+    // ******* Time Tags from signal source (optional feature) *******
+    std::vector<gr::tag_t> tags_vec;
+    this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + 1);  // telemetry decoder consumes symbols one-by-one
+    if (!tags_vec.empty())
+        {
+            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 << "Old tow: " << d_current_timetag.tow_ms << " new tow: " << timetag->tow_ms << "\n";
+                                    d_current_timetag = *timetag;
+                                    d_valid_timetag = true;
+                                }
+                            else
+                                {
+                                    std::cout << "hash code not match\n";
+                                }
+                        }
+                    catch (const boost::bad_any_cast &e)
+                        {
+                            std::cout << "msg Bad any_cast: " << e.what();
+                        }
+                }
+        }
+    else
+        {
+            if (d_valid_timetag == true)
+                {
+                    // propagate timetag to current symbol
+                    // todo: tag rx_time is set only in the time channel. The tracking tag does not have valid rx_time (it is not required since it is associated to the current symbol)
+                    // d_current_timetag.rx_time+=d_PRN_code_period_ms
+                    d_current_timetag.tow_ms += d_PRN_code_period_ms;
+                    if (d_current_timetag.tow_ms >= 604800000)
+                        {
+                            d_current_timetag.tow_ms -= 604800000;
+                            d_current_timetag.week++;
+                        }
+                }
+        }
+    // ***********************************************
+
     consume_each(1);
     d_flag_preamble = false;
 
@@ -873,6 +918,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
                                         d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW5() * 1000.0);
                                         d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
                                         d_inav_nav.set_TOW5_flag(false);
+                                        // timetag debug
+                                        if (d_valid_timetag == true)
+                                            {
+                                                int decoder_delay_ms = static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
+                                                int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms;
+                                                if (rx_tow_at_preamble < 0)
+                                                    {
+                                                        rx_tow_at_preamble += 604800000;
+                                                    }
+                                                uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000);  // floor to integer number of seconds
+                                                std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n";
+                                            }
                                     }
 
                                 else if (d_inav_nav.is_TOW6_set() == true)  // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
@@ -881,6 +938,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
                                         d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW6() * 1000.0);
                                         d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
                                         d_inav_nav.set_TOW6_flag(false);
+                                        // timetag debug
+                                        if (d_valid_timetag == true)
+                                            {
+                                                int decoder_delay_ms = static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
+                                                int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms;
+                                                if (rx_tow_at_preamble < 0)
+                                                    {
+                                                        rx_tow_at_preamble += 604800000;
+                                                    }
+                                                uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000);  // floor to integer number of seconds
+                                                std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n";
+                                            }
                                     }
                                 else if (d_inav_nav.is_TOW0_set() == true)  // page 0 arrived and decoded
                                     {
@@ -888,6 +957,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
                                         d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW0() * 1000.0);
                                         d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
                                         d_inav_nav.set_TOW0_flag(false);
+                                        // timetag debug
+                                        if (d_valid_timetag == true)
+                                            {
+                                                int decoder_delay_ms = static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
+                                                int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms;
+                                                if (rx_tow_at_preamble < 0)
+                                                    {
+                                                        rx_tow_at_preamble += 604800000;
+                                                    }
+                                                uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000);  // floor to integer number of seconds
+                                                std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n";
+                                            }
                                     }
                                 else
                                     {

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h

@@ -26,6 +26,7 @@
 #include "galileo_inav_message.h"     // for Galileo_Inav_Message
 #include "gnss_block_interface.h"     // for gnss_shared_ptr (adapts smart pointer type to GNU Radio version)
 #include "gnss_satellite.h"           // for Gnss_Satellite
+#include "gnss_time.h"                // for GnssTime
 #include "nav_message_packet.h"       // for Nav_Message_Packet
 #include "tlm_conf.h"                 // for Tlm_Conf
 #include <boost/circular_buffer.hpp>  // for boost::circular_buffer
@@ -143,6 +144,8 @@ private:
     bool d_enable_navdata_monitor;
     bool d_dump_crc_stats;
     bool d_enable_reed_solomon_inav;
+    bool d_valid_timetag;
+    GnssTime d_current_timetag;
 };
 
 

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -2021,8 +2021,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
                 {
                     if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr<GnssTime>).hash_code())
                         {
-                            //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing ";
+                            // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing ";
-                            //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing ";
+                            // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing ";
                             const std::shared_ptr<GnssTime> last_timetag = boost::any_cast<const std::shared_ptr<GnssTime>>(pmt::any_ref(it->value));
                             d_last_timetag = *last_timetag;
                             d_last_timetag_samplecounter = it->offset;
@@ -2055,6 +2055,27 @@ 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_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked;
             *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 = 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 0;