mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-04 09:13:05 +00:00
Code Issues Releases Wiki Activity

Merge branch 'fix-glonass-velocity-bug' of https://github.com/vladisslav2011/gnss-sdr into vladisslav2011-fix-glonass-velocity-bug

Browse Source
This commit is contained in:
Carles Fernandez
2022-05-08 20:39:41 +02:00
parent 7cc72faee8 361c3b9166
commit 8b99459c82
8 changed files with 36 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@@ -129,7 +129,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc
d_code_phase_step_chips(0.0),
d_carrier_doppler_hz(0.0),
d_carrier_frequency_hz(0.0),
d_carrier_doppler_old_hz(0.0),
d_carrier_frequency_old_hz(0.0),
d_carrier_phase_step_rad(0.0),
d_acc_carrier_phase_cycles(0.0),
d_code_phase_samples(0.0),
@@ -682,15 +682,16 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
d_carrier_frequency_old_hz = d_carrier_frequency_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_frequency_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_doppler_hz = d_carrier_frequency_hz - DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
d_pll_to_dll_assist_secs_Ti = (d_carrier_frequency_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
// code Doppler frequency update
d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_CPS + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L1_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_CPS + (((d_carrier_frequency_hz - d_carrier_frequency_old_hz) * GLONASS_L1_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
// ################## DLL ##########################################################
// DLL discriminator
@@ -716,12 +717,12 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_frequency_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]

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

@@ -161,7 +161,7 @@ private:
double d_code_phase_step_chips;
double d_carrier_doppler_hz;
double d_carrier_frequency_hz;
double d_carrier_doppler_old_hz;
double d_carrier_frequency_old_hz;
double d_carrier_phase_step_rad;
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;

15
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@@ -127,7 +127,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc
d_code_phase_step_chips(0.0),
d_carrier_doppler_hz(0.0),
d_carrier_frequency_hz(0.0),
d_carrier_doppler_old_hz(0.0),
d_carrier_frequency_old_hz(0.0),
d_carrier_phase_step_rad(0.0),
d_acc_carrier_phase_cycles(0.0),
d_code_phase_samples(0.0),
@@ -682,15 +682,16 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
d_carrier_frequency_old_hz = d_carrier_frequency_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_frequency_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_doppler_hz = d_carrier_frequency_hz - DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
d_pll_to_dll_assist_secs_Ti = (d_carrier_frequency_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
// code Doppler frequency update
d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_CPS + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L1_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_CPS + (((d_carrier_frequency_hz - d_carrier_frequency_old_hz) * GLONASS_L1_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
// ################## DLL ##########################################################
// DLL discriminator
@@ -716,12 +717,12 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_frequency_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]

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

@@ -163,7 +163,7 @@ private:
double d_code_phase_step_chips;
double d_carrier_doppler_hz;
double d_carrier_frequency_hz;
double d_carrier_doppler_old_hz;
double d_carrier_frequency_old_hz;
double d_carrier_phase_step_rad;
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;

15
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
View File

@@ -126,7 +126,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc
d_code_phase_step_chips(0.0),
d_carrier_doppler_hz(0.0),
d_carrier_frequency_hz(0.0),
d_carrier_doppler_old_hz(0.0),
d_carrier_frequency_old_hz(0.0),
d_carrier_phase_step_rad(0.0),
d_acc_carrier_phase_cycles(0.0),
d_code_phase_samples(0.0),
@@ -680,15 +680,16 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
d_carrier_frequency_old_hz = d_carrier_frequency_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_frequency_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_doppler_hz = d_carrier_frequency_hz - DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
d_pll_to_dll_assist_secs_Ti = (d_carrier_frequency_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
// code Doppler frequency update
d_code_freq_chips = GLONASS_L2_CA_CODE_RATE_CPS + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L2_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
d_code_freq_chips = GLONASS_L2_CA_CODE_RATE_CPS + (((d_carrier_frequency_hz - d_carrier_frequency_old_hz) * GLONASS_L2_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
// ################## DLL ##########################################################
// DLL discriminator
@@ -714,12 +715,12 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
//################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_frequency_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
//################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]

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

@@ -159,7 +159,7 @@ private:
double d_code_phase_step_chips;
double d_carrier_doppler_hz;
double d_carrier_frequency_hz;
double d_carrier_doppler_old_hz;
double d_carrier_frequency_old_hz;
double d_carrier_phase_step_rad;
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;

15
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
View File

@@ -125,7 +125,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc
d_code_phase_step_chips(0.0),
d_carrier_doppler_hz(0.0),
d_carrier_frequency_hz(0.0),
d_carrier_doppler_old_hz(0.0),
d_carrier_frequency_old_hz(0.0),
d_carrier_phase_step_rad(0.0),
d_acc_carrier_phase_cycles(0.0),
d_code_phase_samples(0.0),
@@ -679,15 +679,16 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
d_carrier_frequency_old_hz = d_carrier_frequency_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_frequency_hz = d_carrier_loop_filter.get_carrier_error(0.0, static_cast<float>(d_carr_phase_error_secs_Ti), static_cast<float>(CURRENT_INTEGRATION_TIME_S));
d_carrier_doppler_hz = d_carrier_frequency_hz - DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
d_pll_to_dll_assist_secs_Ti = (d_carrier_frequency_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch;
// code Doppler frequency update
d_code_freq_chips = GLONASS_L2_CA_CODE_RATE_CPS + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L2_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
d_code_freq_chips = GLONASS_L2_CA_CODE_RATE_CPS + (((d_carrier_frequency_hz - d_carrier_frequency_old_hz) * GLONASS_L2_CA_CODE_RATE_CPS) / d_glonass_freq_ch);
// ################## DLL ##########################################################
// DLL discriminator
@@ -713,12 +714,12 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_frequency_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]

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

@@ -158,7 +158,7 @@ private:
double d_code_phase_step_chips;
double d_carrier_doppler_hz;
double d_carrier_frequency_hz;
double d_carrier_doppler_old_hz;
double d_carrier_frequency_old_hz;
double d_carrier_phase_step_rad;
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;