mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-12-12 03:28:06 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
pedromiguelcp
2025-08-06 10:30:37 +01:00
committed by Carles Fernandez
parent 629656c9d3
commit 00b9632873
9 changed files with 45 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -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;

8
src/algorithms/PVT/libs/vtl_core.cc Executable file → Normal file
View File

@@ -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;

2
src/algorithms/PVT/libs/vtl_data.cc
View File

@@ -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;

2
src/algorithms/PVT/libs/vtl_data.h
View File

@@ -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

2
src/algorithms/libs/trackingcmd.h
View File

@@ -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;

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

@@ -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;
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<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 + 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;
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<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 + 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));

2
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
View File

@@ -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;

17
utils/python/dll_pll_veml_plot_sample.py
View File

@@ -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()

11
utils/python/lib/dll_pll_veml_read_tracking_dump.py
View File

@@ -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