diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index b4dda46dc..ffaaf7588 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1667,7 +1667,7 @@ bool Rtklib_Solver::get_PVT(const std::map &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; diff --git a/src/algorithms/PVT/libs/vtl_core.cc b/src/algorithms/PVT/libs/vtl_core.cc old mode 100755 new mode 100644 index b5ee689ba..a03ed1c01 --- a/src/algorithms/PVT/libs/vtl_core.cc +++ b/src/algorithms/PVT/libs/vtl_core.cc @@ -443,13 +443,13 @@ 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 trk_cmd.carrier_freq_rate_hz_s = 0; // pseudorange acceleration // DLL - trk_cmd.enable_dll_vtl_feedack = rtk_data.enable_VDLL(aidx(i)); // VDLL + trk_cmd.enable_dll_vtl_feedack = rtk_data.enable_VDLL(aidx(i)); // VDLL trk_cmd.dll_vtl_freq_hz = code_freq - ekf_updt_comp_Z(aidx(i) + N_ch) * range_factor; // pseudorange rate trk_cmd_outs.push_back(trk_cmd); @@ -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; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index e67a88be5..abe9856d2 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/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; diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 30a08c185..1b9907755 100644 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/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 diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index c4cc342c4..c0d021045 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/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; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2b9e603b9..fa5aa572f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/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(this->nitems_read(0) - d_sample_counter_VTL) / d_trk_parameters.fs_in; - 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; + double pvt_trk_diff_time_s = static_cast(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 + 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(this->nitems_read(0) - d_sample_counter_VTL) / 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; + double pvt_trk_diff_time_s = static_cast(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 + 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(&tmp_float), sizeof(float)); tmp_double = static_cast(this->nitems_read(0) + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PVT TRK sample diff + tmp_double = static_cast(this->nitems_read(0) - d_PVT_sample_counter); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index fff7d766d..30325b432 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/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; diff --git a/utils/python/dll_pll_veml_plot_sample.py b/utils/python/dll_pll_veml_plot_sample.py index c7eac6a52..92e309c51 100644 --- a/utils/python/dll_pll_veml_plot_sample.py +++ b/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() diff --git a/utils/python/lib/dll_pll_veml_read_tracking_dump.py b/utils/python/lib/dll_pll_veml_read_tracking_dump.py index 98cb82906..f6ff70fa3 100644 --- a/utils/python/lib/dll_pll_veml_read_tracking_dump.py +++ b/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