diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index c2edc6c5c..b66a78848 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -128,21 +128,33 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip - d_ca_code = static_cast(volk_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 4) * sizeof(gr_complex), volk_get_alignment())); - - d_very_early_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_early_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_prompt_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_late_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_very_late_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_carr_sign = static_cast(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment())); + d_ca_code = static_cast(volk_malloc((2*Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment())); // correlator outputs (scalar) - d_Very_Early = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); - d_Early = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); - d_Prompt = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); - d_Late = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); - d_Very_Late = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); + d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late + d_correlator_outs = static_cast(volk_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + // map memory pointers of correlator outputs + d_Very_Early=&d_correlator_outs[0]; + d_Early=&d_correlator_outs[1]; + d_Prompt=&d_correlator_outs[2]; + d_Late=&d_correlator_outs[3]; + d_Very_Late=&d_correlator_outs[4]; + + d_local_code_shift_chips = static_cast(volk_malloc(d_n_correlator_taps*sizeof(float), volk_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_very_early_late_spc_chips*2.0; + d_local_code_shift_chips[1] = - d_very_early_late_spc_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_very_early_late_spc_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_chips*2.0; + + d_correlation_length_samples=d_vector_length; + + multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); //--- Initializations ------------------------------ // Initial code frequency basis of NCO @@ -198,18 +210,19 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter - // generate local reference ALWAYS starting at chip 2 (2 samples per chip) - galileo_e1_code_gen_complex_sampled(&d_ca_code[2], + // generate local reference ALWAYS starting at chip 1 (2 samples per chip) + galileo_e1_code_gen_complex_sampled(d_ca_code, d_acquisition_gnss_synchro->Signal, false, d_acquisition_gnss_synchro->PRN, 2 * Galileo_E1_CODE_CHIP_RATE_HZ, 0); - // Fill head and tail - d_ca_code[0] = d_ca_code[static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS)]; - d_ca_code[1] = d_ca_code[static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 1)]; - d_ca_code[static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 2)] = d_ca_code[2]; - d_ca_code[static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 3)] = d_ca_code[3]; + + multicorrelator_cpu.set_local_code_and_taps(static_cast(2*Galileo_E1_B_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0.0; @@ -235,77 +248,16 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } - -void galileo_e1_dll_pll_veml_tracking_cc::update_local_code() -{ - double tcode_half_chips; - double rem_code_phase_half_chips; - int associated_chip_index; - int code_length_half_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2; - double code_phase_step_chips; - double code_phase_step_half_chips; - int early_late_spc_samples; - int very_early_late_spc_samples; - int epl_loop_length_samples; - - // unified loop for VE, E, P, L, VL code vectors - code_phase_step_chips = d_code_freq_chips / (static_cast(d_fs_in)); - code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); - - rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in); - tcode_half_chips = - rem_code_phase_half_chips; - - early_late_spc_samples = std::round(d_early_late_spc_chips / code_phase_step_chips); - very_early_late_spc_samples = std::round(d_very_early_late_spc_chips / code_phase_step_chips); - - epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples * 2; - - for (int i = 0; i < epl_loop_length_samples; i++) - { - associated_chip_index = 2 + std::round(std::fmod(tcode_half_chips - 2 * d_very_early_late_spc_chips, code_length_half_chips)); - d_very_early_code[i] = d_ca_code[associated_chip_index]; - tcode_half_chips = tcode_half_chips + code_phase_step_half_chips; - } - memcpy(d_early_code, &d_very_early_code[very_early_late_spc_samples - early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_prompt_code, &d_very_early_code[very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_late_code, &d_very_early_code[very_early_late_spc_samples + early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_very_late_code, &d_very_early_code[2 * very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); -} - - -void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier() -{ - float sin_f, cos_f; - float phase_step_rad = static_cast(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); - int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad); - int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad); - - for(int i = 0; i < d_current_prn_length_samples; i++) - { - gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f); - d_carr_sign[i] = std::complex(cos_f, -sin_f); - phase_rad_i += phase_step_rad_i; - } -} - galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { d_dump_file.close(); - volk_free(d_very_early_code); - volk_free(d_early_code); - volk_free(d_prompt_code); - volk_free(d_late_code); - volk_free(d_very_late_code); - volk_free(d_carr_sign); - volk_free(d_Very_Early); - volk_free(d_Early); - volk_free(d_Prompt); - volk_free(d_Late); - volk_free(d_Very_Late); + volk_free(d_local_code_shift_chips); + volk_free(d_correlator_outs); volk_free(d_ca_code); delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); } @@ -346,24 +298,19 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items, gr_vec const gr_complex* in = (gr_complex*) input_items[0]; Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; - // Generate local code and carrier replicas (using \hat{f}_d(k-1)) - update_local_code(); - update_local_carrier(); + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); - // perform carrier wipe-off and compute Very Early, Early, Prompt, Late and Very Late correlation - d_correlator.Carrier_wipeoff_and_VEPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_very_early_code, - d_early_code, - d_prompt_code, - d_late_code, - d_very_late_code, - d_Very_Early, - d_Early, - d_Prompt, - d_Late, - d_Very_Late); + double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + double code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); + double rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0*d_code_freq_chips / d_fs_in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + carr_phase_step_rad, + rem_code_phase_half_chips, + code_phase_step_half_chips, + d_correlation_length_samples); // ################## PLL ########################################################## // PLL discriminator diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index 2b2642843..5d073ca6f 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -41,7 +41,7 @@ #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_PLL_filter.h" -#include "correlator.h" +#include "cpu_multicorrelator.h" class galileo_e1_dll_pll_veml_tracking_cc; @@ -123,17 +123,16 @@ private: long d_if_freq; long d_fs_in; + //Integration period in samples + int d_correlation_length_samples; + int d_n_correlator_taps; double d_early_late_spc_chips; double d_very_early_late_spc_chips; gr_complex* d_ca_code; - - gr_complex* d_very_early_code; - gr_complex* d_early_code; - gr_complex* d_prompt_code; - gr_complex* d_late_code; - gr_complex* d_very_late_code; - gr_complex* d_carr_sign; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; gr_complex *d_Very_Early; gr_complex *d_Early; @@ -153,9 +152,6 @@ private: double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; - // correlator - Correlator d_correlator; - // tracking vars double d_code_freq_chips; double d_carrier_doppler_hz;