feat: add log PVT(VTL)- TRK timing offset

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1667,7 +1667,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     vtl_data->CN0_dB_hz(ch_id) = gnss_observables_iter->second.CN0_dB_hz;
 
                                     // observations
-                                    vtl_data->ch_sample_counter(ch_id) = gnss_observables_iter->second.Tracking_sample_counter;
+                                    vtl_data->PVT_sample_counter(ch_id) = gnss_observables_iter->second.Tracking_sample_counter;
                                     // pseudorange
                                     vtl_data->obs_pr(ch_id) = gnss_observables_iter->second.Pseudorange_m;
 

src/algorithms/PVT/libs/vtl_core.cc

@@ -443,7 +443,7 @@ void Vtl_Core::send_vtl_feedback(const Vtl_Data &rtk_data)
             // channel info
             trk_cmd.channel_id = aidx(i);
             trk_cmd.prn_id = rtk_data.sv_id(aidx(i));
-            trk_cmd.ch_sample_counter = rtk_data.ch_sample_counter(aidx(i));
+            trk_cmd.PVT_sample_counter = rtk_data.PVT_sample_counter(aidx(i));
             // PLL
             trk_cmd.enable_pll_vtl_feedack = rtk_data.enable_VPLL(aidx(i));
             trk_cmd.pll_vtl_freq_hz = -ekf_updt_comp_Z(aidx(i) + N_ch) / carrier_lambda;  // pseudorange rate
@@ -460,7 +460,7 @@ void Vtl_Core::send_vtl_feedback(const Vtl_Data &rtk_data)
                 {
                     // channel info
                     trk_cmd.channel_id = rtk_data.rx_ch2(aidx(i));
-                    trk_cmd.ch_sample_counter = rtk_data.ch2_sample_counter(aidx(i));
+                    trk_cmd.PVT_sample_counter = rtk_data.ch2_sample_counter(aidx(i));
                     // PLL
                     trk_cmd.pll_vtl_freq_hz = -ekf_updt_comp_Z(aidx(i) + N_ch) / Lambda_GPS_L5;  // pseudorange rate
                     trk_cmd.carrier_freq_rate_hz_s = 0;                                          // pseudorange acceleration
@@ -478,7 +478,7 @@ void Vtl_Core::send_vtl_feedback(const Vtl_Data &rtk_data)
             // channel info
             trk_cmd.channel_id = oldidx(i);
             trk_cmd.prn_id = rtk_data.sv_id(oldidx(i));
-            trk_cmd.ch_sample_counter = 0;
+            trk_cmd.PVT_sample_counter = 0;
             // PLL
             trk_cmd.enable_pll_vtl_feedack = false;
             trk_cmd.pll_vtl_freq_hz = 0;

src/algorithms/PVT/libs/vtl_data.cc

@@ -40,7 +40,7 @@ void Vtl_Data::init_storage(int N_sv)
     band = arma::vec(N_sv);
     code_freq = arma::vec(N_sv);
     CN0_dB_hz = arma::vec(N_sv);
-    ch_sample_counter = arma::vec(N_sv);
+    PVT_sample_counter = arma::vec(N_sv);
     ch2_sample_counter = arma::vec(N_sv);
     ionoopt = 0;
 

src/algorithms/PVT/libs/vtl_data.h

@@ -59,7 +59,7 @@ public:
     arma::colvec band;        // frequency band
     arma::colvec code_freq;
     arma::colvec CN0_dB_hz;
-    arma::colvec ch_sample_counter;
+    arma::colvec PVT_sample_counter;
     arma::colvec ch2_sample_counter;
     int ionoopt;  // ionosphere option
 

src/algorithms/libs/trackingcmd.h

@@ -33,7 +33,7 @@ public:
     bool enable_smooth_pr = false;
     double pll_vtl_freq_hz = 0.0;
     double dll_vtl_freq_hz = 0.0;
-    double ch_sample_counter = 0.0;
+    double PVT_sample_counter = 0.0;
     double carrier_freq_rate_hz_s = 0.0;
     uint32_t channel_id = 0;
     uint32_t prn_id = 0;

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -625,6 +625,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
     d_VTL_dll_en = false;
     d_prn_id = 0;
     d_W_shift_vtl = 0;
+    d_PVT_sample_counter = 0;
 }
 
 
@@ -675,7 +676,7 @@ void dll_pll_veml_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
                         {
                             gr::thread::scoped_lock lock(d_setlock);
                             d_prn_id = cmd->prn_id;
-                            d_sample_counter_VTL = cmd->ch_sample_counter;
+                            d_PVT_sample_counter = cmd->PVT_sample_counter;
 
                             // PLL
                             d_carr_freq_hz_VTL = cmd->pll_vtl_freq_hz;
@@ -1196,8 +1197,8 @@ void dll_pll_veml_tracking::run_dll_pll()
     // VPLL
     if (d_VTL_pll_en)
         {
-            double delta_t_s = static_cast<double>(this->nitems_read(0) - d_sample_counter_VTL) / d_trk_parameters.fs_in;
+            double pvt_trk_diff_time_s = static_cast<double>(this->nitems_read(0) - d_PVT_sample_counter) / d_trk_parameters.fs_in;  // time propagation of VTL feedback
-            d_carrier_doppler_hz = d_W_vtl * (d_carr_freq_hz_VTL + delta_t_s * d_carr_freq_hz_s_VTL) + (1 - d_W_vtl) * d_carrier_doppler_hz;
+            d_carrier_doppler_hz = d_W_vtl * (d_carr_freq_hz_VTL + pvt_trk_diff_time_s * d_carr_freq_hz_s_VTL) + (1 - d_W_vtl) * d_carrier_doppler_hz;
         }
 
     //    std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n';
@@ -1225,13 +1226,13 @@ void dll_pll_veml_tracking::run_dll_pll()
     // VDLL
     if (d_VTL_dll_en)
         {
-            double delta_t_s = static_cast<double>(this->nitems_read(0) - d_sample_counter_VTL) / d_trk_parameters.fs_in;
+            double pvt_trk_diff_time_s = static_cast<double>(this->nitems_read(0) - d_PVT_sample_counter) / d_trk_parameters.fs_in;
-            d_code_freq_chips = d_W_vtl * (d_code_freq_hz_VTL + delta_t_s * d_code_freq_hz_s_VTL) + (1 - d_W_vtl) * d_code_freq_chips;
+            d_code_freq_chips = d_W_vtl * (d_code_freq_hz_VTL + pvt_trk_diff_time_s * d_code_freq_hz_s_VTL) + (1 - d_W_vtl) * d_code_freq_chips;
         }
 
     if (d_W_vtl > 0)
         {
-            d_W_vtl -= d_W_shift_vtl;
+            d_W_vtl -= d_W_shift_vtl;  // shift weight from VTL feedback to tracking loops
         }
 
     // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
@@ -1569,6 +1570,9 @@ void dll_pll_veml_tracking::log_data()
                     d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
                     tmp_double = static_cast<double>(this->nitems_read(0) + d_current_prn_length_samples);
                     d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
+                    // PVT TRK sample diff
+                    tmp_double = static_cast<double>(this->nitems_read(0) - d_PVT_sample_counter);
+                    d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                     // PRN
                     uint32_t prn_ = d_acquisition_gnss_synchro->PRN;
                     d_dump_file.write(reinterpret_cast<char *>(&prn_), sizeof(uint32_t));

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h

@@ -131,7 +131,7 @@ private:
     double d_carr_freq_hz_s_VTL;
     double d_code_freq_hz_VTL;
     double d_code_freq_hz_s_VTL;
-    double d_sample_counter_VTL;
+    double d_PVT_sample_counter;
     double d_W_vtl;
     double d_carrier_doppler_hz;
     double d_acc_carrier_phase_rad;

utils/python/dll_pll_veml_plot_sample.py

@@ -43,6 +43,7 @@ plot_last_outputs = 0
 channels = 5
 first_channel = 0
 doppler_opt = 1
+PVT_TRK_diff = 0
 settings['numberOfChannels'] = channels
 
 path = '/home/labnav/Desktop/TEST_IRENE/tracking'
@@ -108,3 +109,19 @@ for N in range (1, channels+1):
 
         plt.savefig(os.path.join(fig_path, f'Doppler_freq_ch_{N}.png'))
         plt.show()
+
+    # Plot time offset between PVT and Tracking:
+    if PVT_TRK_diff == 1:
+        if not os.path.exists(fig_path):
+            os.makedirs(fig_path)
+
+        plt.figure()
+        plt.plot(trackResults[N - 1]['prn_start_time_s'],
+                 [x/(sampling_freq/1000) for x in GNSS_tracking[N - 1]['PVT_TRK_diff']
+                 [start_sample:]])
+        plt.xlabel('Time(s)')
+        plt.ylabel('Offset(ms)')
+        plt.title('PVT Tracking Time Offset ' + str(N))
+
+        plt.savefig(os.path.join(fig_path, f'PVT_TRK_diff_{N}.png'))
+        plt.show()

utils/python/lib/dll_pll_veml_read_tracking_dump.py

@@ -52,6 +52,7 @@ def dll_pll_veml_read_tracking_dump (filename):
     v20 = []
     v21 = []
     v22 = []
+    v23 = []
     GNSS_tracking = {}
 
     bytes_shift = 0
@@ -183,8 +184,13 @@ def dll_pll_veml_read_tracking_dump (filename):
                                      f.read(double_size_bytes))[0])
             bytes_shift += double_size_bytes
             f.seek(bytes_shift, 0)
+            # PVT TRK sample diff
+            v22.append(struct.unpack('d',
+                                     f.read(double_size_bytes))[0])
+            bytes_shift += double_size_bytes
+            f.seek(bytes_shift, 0)
             # PRN ->  Satellite ID.
-            v22.append(struct.unpack('I',
+            v23.append(struct.unpack('I',
                                      f.read(unsigned_int_size_bytes))[0])
             bytes_shift += unsigned_int_size_bytes
             f.seek(bytes_shift, 0)
@@ -217,6 +223,7 @@ def dll_pll_veml_read_tracking_dump (filename):
         GNSS_tracking['carrier_lock_test'] = v19
         GNSS_tracking['var1'] = v20
         GNSS_tracking['var2'] = v21
-        GNSS_tracking['PRN'] = v22
+        GNSS_tracking['PVT_TRK_diff'] = v22
+        GNSS_tracking['PRN'] = v23
 
     return GNSS_tracking