From fcd24c69daddee9a7b7e8a91b2d18b84173a4404 Mon Sep 17 00:00:00 2001 From: Gastd Date: Sun, 21 Jan 2018 00:30:31 -0200 Subject: [PATCH] fixbug: initialize the carrier track filter with proper value --- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 8 +++++--- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.h | 1 + .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 16 ++++++++++------ .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.h | 2 ++ 4 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index d6569fb81..adedc967a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -218,6 +218,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc d_preamble_timestamp_s = 0.0; d_carrier_frequency_hz = 0.0; + d_carrier_doppler_old_hz = 0.0; //set_min_output_buffer((long int)300); } @@ -270,13 +271,13 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() // d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); // d_carrier_doppler_hz = d_acq_carrier_doppler_hz; // d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_carrier_frequency_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_carrier_frequency_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN))); d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast(d_fs_in); // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator + d_carrier_loop_filter.initialize(d_carrier_frequency_hz); // The carrier loop filter implements the Doppler accumulator d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) @@ -709,6 +710,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __a // ################## PLL ########################################################## // Update PLL discriminator [rads/Ti -> Secs/Ti] d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output + d_carrier_doppler_old_hz = d_carrier_doppler_hz; // Carrier discriminator filter // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan // Input [s/Ti] -> output [Hz] @@ -716,7 +718,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __a // 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; // code Doppler frequency update - d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); // ################## DLL ########################################################## // DLL discriminator diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h index 1aa7b72bd..fd8c3f181 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h @@ -158,6 +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_phase_step_rad; double d_acc_carrier_phase_cycles; double d_code_phase_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 197e46379..99ce87e3b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -217,6 +217,10 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc d_code_error_filt_chips_Ti = 0.0; d_preamble_timestamp_s = 0.0; d_carr_phase_error_secs_Ti = 0.0; + + d_carrier_frequency_hz = 0.0; + d_carrier_doppler_old_hz = 0.0; + //set_min_output_buffer((long int)300); } @@ -265,13 +269,13 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); - // d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_frequency_hz = d_acq_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)));; + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast(d_fs_in); // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator + d_carrier_loop_filter.initialize(d_carrier_frequency_hz); // The carrier loop filter implements the Doppler accumulator d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) @@ -700,7 +704,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __a // ################## PLL ########################################################## // Update PLL discriminator [rads/Ti -> Secs/Ti] d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / GLONASS_TWO_PI; //prompt output - + d_carrier_doppler_old_hz = d_carrier_doppler_hz; // Carrier discriminator filter // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan // Input [s/Ti] -> output [Hz] @@ -708,7 +712,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __a // 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; // code Doppler frequency update - d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + (((d_carrier_doppler_hz - d_carrier_doppler_old_hz) * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); // ################## DLL ########################################################## // DLL discriminator diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h index 6f83bac0d..5fe82b1b9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h @@ -160,6 +160,8 @@ private: double d_code_freq_chips; 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_phase_step_rad; double d_acc_carrier_phase_cycles; double d_code_phase_samples;