From 9db0990adc2526a17f70bac0c272ecc8c5dea83b Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 6 Apr 2016 14:58:18 +0200 Subject: [PATCH] Deleted obsolete DLL_FLL_PLL GPS tracking. Deleted obsolete correlator.h and migrated all tracking in order to use the newer and optimized cpu_multicorrelator. Code simplification and code cleaning --- conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf | 4 +- conf/gnss-sdr_Hybrid_nsr.conf | 12 +- .../gps_l1_ca_telemetry_decoder_cc.cc | 2 +- .../tracking/adapters/CMakeLists.txt | 1 - .../gps_l1_ca_tcp_connector_tracking.cc | 6 - .../tracking/gnuradio_blocks/CMakeLists.txt | 1 - .../galileo_e1_dll_pll_veml_tracking_cc.cc | 18 +- .../galileo_e1_tcp_connector_tracking_cc.cc | 156 ++++--------- .../galileo_e1_tcp_connector_tracking_cc.h | 17 +- .../galileo_e5a_dll_pll_tracking_cc.cc | 191 ++++++--------- .../galileo_e5a_dll_pll_tracking_cc.h | 26 ++- .../gps_l1_ca_dll_pll_tracking_cc.cc | 220 ++++++------------ .../gps_l1_ca_dll_pll_tracking_cc.h | 25 +- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 172 ++++---------- .../gps_l1_ca_tcp_connector_tracking_cc.h | 28 +-- .../gps_l2_m_dll_pll_tracking_cc.cc | 196 +++++----------- .../gps_l2_m_dll_pll_tracking_cc.h | 24 +- src/algorithms/tracking/libs/CMakeLists.txt | 1 - src/core/receiver/gnss_block_factory.cc | 13 -- 19 files changed, 375 insertions(+), 738 deletions(-) diff --git a/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf b/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf index 89be9e4f8..fba005fbe 100644 --- a/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf +++ b/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf @@ -26,7 +26,7 @@ ControlThread.wait_for_flowgraph=false ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=File_Signal_Source -SignalSource.filename=/datalogger/captures/Galileo_E5ab_IFEN_CTTC_run1.dat +SignalSource.filename=/media/javier/SISTEMA/signals/captura Generador IFEN CTTC Gal E5ab/Galileo_E5ab_IFEN_CTTC_run1.dat SignalSource.item_type=gr_complex SignalSource.sampling_frequency=50000000 SignalSource.freq=1176450000 @@ -76,7 +76,7 @@ Resampler.dump_filename=../data/resampler.dat ;######### CHANNELS GLOBAL CONFIG ############ -Channels_5X.count=4 +Channels_5X.count=1 Channels.in_acquisition=1 Channel.signal=5X diff --git a/conf/gnss-sdr_Hybrid_nsr.conf b/conf/gnss-sdr_Hybrid_nsr.conf index e6435579c..30006e727 100644 --- a/conf/gnss-sdr_Hybrid_nsr.conf +++ b/conf/gnss-sdr_Hybrid_nsr.conf @@ -200,7 +200,7 @@ Acquisition_1C.threshold=0.0075 ;#doppler_max: Maximum expected Doppler shift [Hz] Acquisition_1C.doppler_max=5000 ;#doppler_max: Doppler step in the grid search [Hz] -Acquisition_1C.doppler_step=500 +Acquisition_1C.doppler_step=250 ;######### GALILEO ACQUISITION CONFIG ############ @@ -228,8 +228,8 @@ Acquisition_1B.doppler_step=125 ;######### TRACKING GPS CONFIG ############ -;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. Tracking_1C.item_type=gr_complex @@ -237,7 +237,7 @@ Tracking_1C.item_type=gr_complex Tracking_1C.if=0 ;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] -Tracking_1C.dump=false +Tracking_1C.dump=true ;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. Tracking_1C.dump_filename=../data/epl_tracking_ch_ @@ -246,7 +246,7 @@ Tracking_1C.dump_filename=../data/epl_tracking_ch_ ;# Valid values are: [1,2,4,5,10,20] (integer divisors of the GPS L1 CA bit period (20 ms) ) ;# Longer integration period require more stable front-end LO -Tracking_1C.extend_correlation_ms=10 +Tracking_1C.extend_correlation_ms=1 ;#pll_bw_hz: PLL loop filter bandwidth [Hz] @@ -265,7 +265,7 @@ Tracking_1C.order=3; ;######### TRACKING GALILEO CONFIG ############ -;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. Tracking_1B.item_type=gr_complex diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index be09af2be..b4d00ffd2 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -234,7 +234,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i d_symbol_accumulator += in[0][0].Prompt_I; // accumulate the input value in d_symbol_accumulator d_symbol_accumulator_counter += in[0][0].correlation_length_ms; } - if (d_symbol_accumulator_counter == 20) + if (d_symbol_accumulator_counter >= 20) { if (d_symbol_accumulator > 0) { //symbol to bit diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 3d146f646..c9841612c 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -24,7 +24,6 @@ endif(ENABLE_CUDA) set(TRACKING_ADAPTER_SOURCES galileo_e1_dll_pll_veml_tracking.cc galileo_e1_tcp_connector_tracking.cc - gps_l1_ca_dll_fll_pll_tracking.cc gps_l1_ca_dll_pll_tracking.cc gps_l1_ca_dll_pll_c_aid_tracking.cc gps_l1_ca_tcp_connector_tracking.cc diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc index f21e73b94..0ca1c9142 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc @@ -59,8 +59,6 @@ GpsL1CaTcpConnectorTracking::GpsL1CaTcpConnectorTracking( std::string dump_filename; std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; float early_late_space_chips; size_t port_ch0; item_type = configuration->property(role + ".item_type",default_item_type); @@ -68,8 +66,6 @@ GpsL1CaTcpConnectorTracking::GpsL1CaTcpConnectorTracking( fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); port_ch0 = configuration->property(role + ".port_ch0", 2060); std::string default_dump_filename = "./track_ch"; @@ -87,8 +83,6 @@ GpsL1CaTcpConnectorTracking::GpsL1CaTcpConnectorTracking( queue_, dump, dump_filename, - pll_bw_hz, - dll_bw_hz, early_late_space_chips, port_ch0); } diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 3a583b7b0..1e2fe27b2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -25,7 +25,6 @@ endif(ENABLE_CUDA) set(TRACKING_GR_BLOCKS_SOURCES galileo_e1_dll_pll_veml_tracking_cc.cc galileo_e1_tcp_connector_tracking_cc.cc - gps_l1_ca_dll_fll_pll_tracking_cc.cc gps_l1_ca_dll_pll_tracking_cc.cc gps_l1_ca_tcp_connector_tracking_cc.cc galileo_e5a_dll_pll_tracking_cc.cc 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 46f25fc08..d1bb94004 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 @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include "galileo_e1_signal_processing.h" #include "tracking_discriminators.h" #include "lock_detectors.h" @@ -129,11 +129,11 @@ 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_gnsssdr_malloc((2*Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_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_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment())); + 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); @@ -145,7 +145,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_Late = &d_correlator_outs[3]; d_Very_Late = &d_correlator_outs[4]; - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + 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; @@ -253,9 +253,9 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { d_dump_file.close(); - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - volk_gnsssdr_free(d_ca_code); + volk_free(d_local_code_shift_chips); + volk_free(d_correlator_outs); + volk_free(d_ca_code); delete[] d_Prompt_buffer; multicorrelator_cpu.free(); @@ -526,7 +526,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items, gr_vec tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception writing trk dump file " << e.what() << std::endl; } @@ -560,7 +560,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl; } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index e84b01fc1..28e6e4e29 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -133,22 +133,33 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_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_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_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_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); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -198,18 +209,19 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking() d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - // 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, + 2 * Galileo_E1_CODE_CHIP_RATE_HZ, 0); - // Fill head and tail - d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)]; - d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+1)]; - d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+2)] = d_ca_code[2]; - d_ca_code[(int)(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,78 +247,17 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking() } -void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_code() -{ - double tcode_half_chips; - float rem_code_phase_half_chips; - int associated_chip_index; - int code_length_half_chips = (int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS); - 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 = ((double)d_code_freq_chips) / ((double)d_fs_in); - code_phase_step_half_chips = (2.0*(double)d_code_freq_chips) / ((double)d_fs_in); - - rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in); - tcode_half_chips = -(double)rem_code_phase_half_chips; - - early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); - very_early_late_spc_samples = 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 + round(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[2*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_Tcp_Connector_Tracking_cc::update_local_carrier() -{ - float phase_rad, phase_step_rad; - // Compute the carrier phase step for the K-1 carrier Doppler estimation - phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; - // Initialize the carrier phase with the remnant carrier phase of the K-2 loop - phase_rad = d_rem_carr_phase_rad; - for(int i = 0; i < d_current_prn_length_samples; i++) - { - d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); - phase_rad += phase_step_rad; - } -} - - Galileo_E1_Tcp_Connector_Tracking_cc::~Galileo_E1_Tcp_Connector_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_ca_code); - delete[] d_Prompt_buffer; + volk_free(d_ca_code); + volk_free(d_local_code_shift_chips); + volk_free(d_correlator_outs); d_tcp_com.close_tcp_connection(d_port); + multicorrelator_cpu.free(); } @@ -346,24 +297,19 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve 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 Early, Prompt and 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); // ################## TCP CONNECTOR ########################################################## //! Variable used for control @@ -588,7 +534,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve tmp_double = (double)(d_sample_counter+d_current_prn_length_samples); d_dump_file.write((char*)&tmp_double, sizeof(double)); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception writing trk dump file " << e.what(); } @@ -621,7 +567,7 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel) d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h index e57586d61..358ef32a3 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h @@ -46,7 +46,8 @@ #include #include "concurrent_queue.h" #include "gnss_synchro.h" -#include "correlator.h" +#include +#include "cpu_multicorrelator.h" #include "tcp_communication.h" @@ -127,18 +128,13 @@ private: long d_if_freq; long d_fs_in; + int d_correlation_length_samples; + int d_n_correlator_taps; float d_early_late_spc_chips; float 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; - gr_complex *d_Very_Early; gr_complex *d_Early; gr_complex *d_Prompt; @@ -155,7 +151,9 @@ private: float d_acq_carrier_doppler_hz; // correlator - Correlator d_correlator; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; // tracking vars double d_code_freq_chips; @@ -172,7 +170,6 @@ private: //PRN period in samples int d_current_prn_length_samples; int d_next_prn_length_samples; - //double d_sample_counter_seconds; //processing samples counters unsigned long int d_sample_counter; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index 09c4b0d45..129204e85 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -137,20 +137,36 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc( // Initialization of local code replica // Get space for a vector with the E5a primary code replicas sampled 1x/chip - d_codeQ = new gr_complex[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS) + 2]; - d_codeI = new gr_complex[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS) + 2]; + d_codeQ = static_cast(volk_malloc(Galileo_E5a_CODE_LENGTH_CHIPS * sizeof(gr_complex), volk_get_alignment())); + d_codeI = static_cast(volk_malloc(Galileo_E5a_CODE_LENGTH_CHIPS * sizeof(gr_complex), volk_get_alignment())); - d_early_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_prompt_code = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - d_prompt_data_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())); + // correlator Q outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, 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); + } - // correlator outputs (complex number) - d_Early = gr_complex(0, 0); - d_Prompt = gr_complex(0, 0); - d_Late = gr_complex(0, 0); - d_Prompt_data = gr_complex(0, 0); + std::cout<<"1"<(volk_malloc(d_n_correlator_taps * sizeof(float), volk_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu_Q.init(2 * d_vector_length, d_n_correlator_taps); + + std::cout<<"1"<(volk_malloc(sizeof(gr_complex), volk_get_alignment())); + *d_Single_Prompt_data = gr_complex(0,0); + multicorrelator_cpu_I.init(2 * d_vector_length, 1); // single correlator for data channel //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -196,6 +212,8 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc( d_state = 0; systemName["E"] = std::string("Galileo"); + + std::cout<<"end constructor"<PRN, sig); - d_codeQ[0] = d_codeQ[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS)]; - d_codeQ[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeQ[1]; + galileo_e5_a_code_gen_complex_primary(d_codeQ, d_acquisition_gnss_synchro->PRN, sig); strcpy(sig,"5I"); - galileo_e5_a_code_gen_complex_primary(&d_codeI[1], d_acquisition_gnss_synchro->PRN, sig); - d_codeI[0] = d_codeI[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS)]; - d_codeI[static_cast(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeI[1]; + galileo_e5_a_code_gen_complex_primary(d_codeI, d_acquisition_gnss_synchro->PRN, sig); d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0; @@ -299,6 +317,8 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking() LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + + std::cout<<"end start trk"<(Galileo_E5a_CODE_LENGTH_CHIPS); - double code_phase_step_chips; - int early_late_spc_samples; - int epl_loop_length_samples; - - // unified loop for E, P, L code vectors - code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); - tcode_chips = -rem_code_phase_chips; - - // Alternative EPL code generation (40% of speed improvement!) - early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); - epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2; - - for (int i = 0; i < epl_loop_length_samples; i++) - { - associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); - associated_chip_index_data = 1 + round(fmod(tcode_chips, code_length_chips)); - d_early_code[i] = d_codeQ[associated_chip_index]; - d_prompt_data_code[i] = d_codeI[associated_chip_index_data]; - tcode_chips = tcode_chips + code_phase_step_chips; - } - memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex)); - -} - - -void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_carrier() -{ - float sin_f, cos_f; - float phase_step_rad = static_cast(2.0 * 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; - } -} - - int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -512,59 +482,50 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ //Generate local code and carrier replicas (using \hat{f}_d(k-1)) if (d_integration_counter == 0) { - update_local_code(); - update_local_carrier(); // Reset accumulated values d_Early = gr_complex(0,0); d_Prompt = gr_complex(0,0); d_Late = gr_complex(0,0); } - gr_complex single_early; - gr_complex single_prompt; - gr_complex single_late; // perform carrier wipe-off and compute Early, Prompt and Late // correlation of 1 primary code - d_correlator.Carrier_wipeoff_and_EPL_volk_IQ(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_prompt_data_code, - &single_early, - &single_prompt, - &single_late, - &d_Prompt_data); + + multicorrelator_cpu_Q.set_local_code_and_taps(d_current_prn_length_samples, d_codeQ, d_local_code_shift_chips); + multicorrelator_cpu_I.set_local_code_and_taps(d_current_prn_length_samples, d_codeI, &d_local_code_shift_chips[1]); + + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu_Q.set_input_output_vectors(d_correlator_outs,in); + multicorrelator_cpu_I.set_input_output_vectors(d_Single_Prompt_data,in); + + double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + double code_phase_step_chips = d_code_freq_chips / (static_cast(d_fs_in)); + double rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); + multicorrelator_cpu_Q.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + carr_phase_step_rad, + rem_code_phase_chips, + code_phase_step_chips, + d_current_prn_length_samples); + + multicorrelator_cpu_I.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + carr_phase_step_rad, + rem_code_phase_chips, + code_phase_step_chips, + d_current_prn_length_samples); + // Accumulate results (coherent integration since there are no bit transitions in pilot signal) - d_Early += single_early * sec_sign_Q; - d_Prompt += single_prompt * sec_sign_Q; - d_Late += single_late * sec_sign_Q; + d_Early += (*d_Single_Early) * sec_sign_Q; + d_Prompt += (*d_Single_Prompt) * sec_sign_Q; + d_Late += (*d_Single_Late) * sec_sign_Q; + d_Prompt_data=(*d_Single_Prompt_data); d_Prompt_data *= sec_sign_I; d_integration_counter++; - // check for samples consistency (this should be done before in the receiver / here only if the source is a file) - if (std::isnan((d_Prompt).real()) == true or std::isnan((d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) - { - const int samples_available = ninput_items[0]; - d_sample_counter = d_sample_counter + samples_available; - LOG(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; - consume_each(samples_available); - - // make an output to not stop the rest of the processing blocks - current_synchro_data.Prompt_I = 0.0; - current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); - current_synchro_data.Carrier_phase_rads = 0.0; - current_synchro_data.Code_phase_secs = 0.0; - current_synchro_data.CN0_dB_hz = 0.0; - current_synchro_data.Flag_valid_tracking = false; - - *out[0] = current_synchro_data; - - return 1; - } // ################## PLL ########################################################## // PLL discriminator if (d_integration_counter == d_current_ti_ms) @@ -823,7 +784,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure & e) { LOG(WARNING) << "Exception writing trk dump file " << e.what(); } @@ -857,7 +818,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl; } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl; } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h index ae06357c0..ffb98f759 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h @@ -46,7 +46,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_E5a_Dll_Pll_Tracking_cc; @@ -114,8 +114,6 @@ private: float dll_bw_init_hz, int ti_ms, float early_late_space_chips); - void update_local_code(); - void update_local_carrier(); void acquire_secondary(); // tracking configuration vars boost::shared_ptr d_queue; @@ -141,22 +139,23 @@ private: gr_complex* d_codeQ; gr_complex* d_codeI; - gr_complex* d_early_code; - gr_complex* d_late_code; - gr_complex* d_prompt_code; - gr_complex* d_prompt_data_code; - gr_complex* d_carr_sign; - gr_complex d_Early; gr_complex d_Prompt; gr_complex d_Late; gr_complex d_Prompt_data; + gr_complex* d_Single_Early; + gr_complex* d_Single_Prompt; + gr_complex* d_Single_Late; + gr_complex* d_Single_Prompt_data; + + float tmp_E; float tmp_P; float tmp_L; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; + double d_rem_code_phase_chips; double d_rem_carr_phase_rad; // PLL and DLL filter library @@ -167,7 +166,11 @@ private: double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator - Correlator d_correlator; + int d_n_correlator_taps; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu_I; + cpu_multicorrelator multicorrelator_cpu_Q; // tracking vars double d_code_freq_chips; @@ -176,6 +179,9 @@ private: double d_code_phase_samples; double d_acc_code_phase_secs; double d_code_error_filt_secs; + double d_code_phase_step_chips; + double d_carrier_phase_step_rad; + //PRN period in samples int d_current_prn_length_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 26d8fc5ef..0a6b6f76c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -123,21 +123,22 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( // Initialization of local code replica // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment())); - - // Get space for the resampled early / prompt / late local replicas - 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())); - - // space for carrier wipeoff and signal baseband vectors - d_carr_sign = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); + d_ca_code = static_cast(volk_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment())); // correlator outputs (scalar) - 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_n_correlator_taps = 3; // Early, Prompt, and 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); + } + 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_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + multicorrelator_cpu.init(2 * d_vector_length, d_n_correlator_taps); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -169,9 +170,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( systemName["G"] = std::string("GPS"); systemName["S"] = std::string("SBAS"); - - set_relative_rate(1.0/((double)d_vector_length*2)); - d_channel_internal_queue = 0; d_acquisition_gnss_synchro = 0; d_channel = 0; @@ -181,7 +179,8 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( d_acc_carrier_phase_rad = 0.0; d_code_phase_samples = 0.0; d_acc_code_phase_secs = 0.0; - //set_min_output_buffer((long int)300); + + set_relative_rate(1.0/((double)d_vector_length*2)); } @@ -234,9 +233,13 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); - d_ca_code[0] = d_ca_code[static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS)]; - d_ca_code[static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1]; + gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_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; @@ -262,79 +265,21 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } - - - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code() -{ - double tcode_chips; - double rem_code_phase_chips; - int associated_chip_index; - int code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); - double code_phase_step_chips; - int early_late_spc_samples; - int epl_loop_length_samples; - - // unified loop for E, P, L code vectors - code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); - tcode_chips = -rem_code_phase_chips; - - // Alternative EPL code generation (40% of speed improvement!) - early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); - epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2; - for (int i = 0; i < epl_loop_length_samples; i++) - { - associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); - d_early_code[i] = d_ca_code[associated_chip_index]; - tcode_chips = tcode_chips + code_phase_step_chips; - } - - memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex)); -} - - - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier() -{ - float sin_f, cos_f; - float phase_step_rad = static_cast(GPS_TWO_PI) * static_cast( d_if_freq + 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; - } -} - - - - Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc() { d_dump_file.close(); - volk_free(d_prompt_code); - volk_free(d_late_code); - volk_free(d_early_code); - volk_free(d_carr_sign); - volk_free(d_Early); - volk_free(d_Prompt); - volk_free(d_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(); } -int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, +int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars @@ -353,6 +298,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in if (d_enable_tracking == true) { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; // Receiver signal alignment if (d_pull_in == true) { @@ -364,60 +311,31 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_pull_in = false; - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.correlation_length_ms = 1; + current_synchro_data.Flag_valid_symbol_output = false; *out[0] = current_synchro_data; consume_each(samples_offset); //shift input to perform alignment with local replica return 1; } - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - // 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 - d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_Early, - d_Prompt, - d_Late); - - // check for samples consistency (this should be done before in the receiver / here only if the source is a file) - if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) - { - const int samples_available = ninput_items[0]; - d_sample_counter = d_sample_counter + samples_available; - LOG(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; - consume_each(samples_available); - - // make an output to not stop the rest of the processing blocks - current_synchro_data.Prompt_I = 0.0; - current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); - current_synchro_data.Carrier_phase_rads = 0.0; - current_synchro_data.Code_phase_secs = 0.0; - current_synchro_data.CN0_dB_hz = 0.0; - current_synchro_data.Flag_valid_tracking = false; - current_synchro_data.Flag_valid_pseudorange = false; - - *out[0] = current_synchro_data; - return 1; - } + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); // ################## PLL ########################################################## // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI; + // Update PLL discriminator [rads/Ti -> Secs/Ti] + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; //prompt output // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; + // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation @@ -428,7 +346,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // ################## DLL ########################################################## // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti] + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator @@ -448,13 +366,22 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in T_prn_samples = T_prn_seconds * static_cast(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + + //################### PLL COMMANDS ################################################# + //carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + //################### DLL COMMANDS ################################################# + //code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) { // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt d_cn0_estimation_counter++; } else @@ -487,8 +414,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in } } // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); @@ -551,28 +478,30 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in std::cout << tmp_str_stream.rdbuf() << std::flush; } } - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } current_synchro_data.System = {'G'}; current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_symbol_output = false; + current_synchro_data.correlation_length_ms=1; *out[0] = current_synchro_data; } if(d_dump) { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - prompt_I = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); try { @@ -620,10 +549,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_current_prn_length_samples; //count for the processed samples - if((noutput_items == 0) || (ninput_items[0] == 0)) - { - LOG(WARNING) << "noutput_items = 0"; - } +// if((noutput_items == 0) || (ninput_items[0] == 0)) +// { +// LOG(WARNING) << "noutput_items = 0"; +// } return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false } @@ -654,14 +583,11 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) } } - - void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel_queue(concurrent_queue *channel_internal_queue) { d_channel_internal_queue = channel_internal_queue; } - void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { d_acquisition_gnss_synchro = p_gnss_synchro; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index 2ff14ad71..6808aec5a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -46,7 +46,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 Gps_L1_Ca_Dll_Pll_Tracking_cc; @@ -105,8 +105,6 @@ private: float pll_bw_hz, float dll_bw_hz, float early_late_space_chips); - void update_local_code(); - void update_local_carrier(); // tracking configuration vars boost::shared_ptr d_queue; @@ -122,19 +120,9 @@ private: double d_early_late_spc_chips; - gr_complex* d_ca_code; - - gr_complex* d_early_code; - gr_complex* d_late_code; - gr_complex* d_prompt_code; - gr_complex* d_carr_sign; - - gr_complex *d_Early; - gr_complex *d_Prompt; - gr_complex *d_Late; - // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; + double d_rem_code_phase_chips; double d_rem_carr_phase_rad; // PLL and DLL filter library @@ -145,11 +133,18 @@ private: double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator - Correlator d_correlator; + int d_n_correlator_taps; + gr_complex* d_ca_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; + // tracking vars double d_code_freq_chips; + double d_code_phase_step_chips; double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; double d_acc_code_phase_secs; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 0ae1f08c5..097fc4a13 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -70,13 +70,11 @@ gps_l1_ca_tcp_connector_make_tracking_cc( boost::shared_ptr queue, bool dump, std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, float early_late_space_chips, size_t port_ch0) { return gps_l1_ca_tcp_connector_tracking_cc_sptr(new Gps_L1_Ca_Tcp_Connector_Tracking_cc(if_freq, - fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, port_ch0)); + fs_in, vector_length, queue, dump, dump_filename, early_late_space_chips, port_ch0)); } @@ -99,8 +97,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc( boost::shared_ptr queue, bool dump, std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, float early_late_space_chips, size_t port_ch0) : gr::block("Gps_L1_Ca_Tcp_Connector_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), @@ -116,10 +112,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc( d_vector_length = vector_length; d_dump_filename = dump_filename; - // Initialize tracking ========================================== - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); - //--- DLL variables -------------------------------------------------------- d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) @@ -131,21 +123,29 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc( // Initialization of local code replica // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment())); - d_carr_sign = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); - - // Get space for the resampled early / prompt / late local replicas - 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())); - - // space for carrier wipeoff and signal baseband vectors - d_carr_sign = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); + d_ca_code = static_cast(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment())); // correlator outputs (scalar) - 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_n_correlator_taps = 3; // 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_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + + 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_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + d_correlation_length_samples=d_vector_length; + + multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -247,16 +247,16 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking() delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - // DLL/PLL filter initialization - 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 1 (1 sample per chip) - gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); - d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; - d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1]; + gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_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; @@ -286,78 +286,19 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking() - -void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_code() -{ - double tcode_chips; - double rem_code_phase_chips; - int associated_chip_index; - int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; - double code_phase_step_chips; - int early_late_spc_samples; - int epl_loop_length_samples; - - // unified loop for E, P, L code vectors - code_phase_step_chips = ((double)d_code_freq_hz) / ((double)d_fs_in); - rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in); - tcode_chips = -rem_code_phase_chips; - - // Alternative EPL code generation (40% of speed improvement!) - early_late_spc_samples = round(d_early_late_spc_chips/code_phase_step_chips); - epl_loop_length_samples = d_current_prn_length_samples+early_late_spc_samples*2; - for (int i = 0; i < epl_loop_length_samples; i++) - { - associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); - d_early_code[i] = d_ca_code[associated_chip_index]; - tcode_chips = tcode_chips + d_code_phase_step_chips; - } - - memcpy(d_prompt_code,&d_early_code[early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); - memcpy(d_late_code,&d_early_code[early_late_spc_samples*2], d_current_prn_length_samples* sizeof(gr_complex)); -} - - - - -void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_carrier() -{ - float phase_rad, phase_step_rad; - - phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; - phase_rad = d_rem_carr_phase_rad; - for(int i = 0; i < d_current_prn_length_samples; i++) - { - d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); - phase_rad += phase_step_rad; - } - d_rem_carr_phase_rad = fmod(phase_rad, GPS_TWO_PI); - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + d_rem_carr_phase_rad; -} - - - - Gps_L1_Ca_Tcp_Connector_Tracking_cc::~Gps_L1_Ca_Tcp_Connector_Tracking_cc() { d_dump_file.close(); - volk_free(d_prompt_code); - volk_free(d_late_code); - volk_free(d_early_code); - volk_free(d_carr_sign); - volk_free(d_Early); - volk_free(d_Prompt); - volk_free(d_Late); - volk_free(d_ca_code); - delete[] d_Prompt_buffer; + volk_free(d_ca_code); + volk_free(d_local_code_shift_chips); + volk_free(d_correlator_outs); d_tcp_com.close_tcp_connection(d_port); + multicorrelator_cpu.free(); } - - - int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -405,42 +346,19 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec // variable code PRN sample block size d_current_prn_length_samples = d_next_prn_length_samples; - 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 Early, Prompt and Late correlation - d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_Early, - d_Prompt, - d_Late); + double carr_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + double rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in); - // check for samples consistency (this should be done before in the receiver / here only if the source is a file) - if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) - { - const int samples_available = ninput_items[0]; - d_sample_counter = d_sample_counter + samples_available; - LOG(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; - consume_each(samples_available); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + carr_phase_step_rad, + rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); - // make an output to not stop the rest of the processing blocks - current_synchro_data.Prompt_I = 0.0; - current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds; - current_synchro_data.Carrier_phase_rads = 0.0; - current_synchro_data.Code_phase_secs = 0.0; - current_synchro_data.CN0_dB_hz = 0.0; - current_synchro_data.Flag_valid_tracking = false; - current_synchro_data.Flag_valid_pseudorange = false; - current_synchro_data.Flag_valid_symbol_output = true; - *out[0] = current_synchro_data; - - return 1; - } //! Variable used for control d_control_id++; @@ -658,7 +576,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double)); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception writing trk dump file " << e.what(); } @@ -693,7 +611,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel) d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h index d272b1639..086850b41 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h @@ -44,9 +44,7 @@ #include #include "concurrent_queue.h" #include "gnss_synchro.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" -#include "correlator.h" +#include "cpu_multicorrelator.h" #include "tcp_communication.h" @@ -62,8 +60,6 @@ gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq, boost::shared_ptr queue, bool dump, std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, float early_late_space_chips, size_t port_ch0); @@ -98,8 +94,6 @@ private: boost::shared_ptr queue, bool dump, std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, float early_late_space_chips, size_t port_ch0); @@ -109,12 +103,8 @@ private: boost::shared_ptr queue, bool dump, std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, float early_late_space_chips, size_t port_ch0); - void update_local_code(); - void update_local_carrier(); // tracking configuration vars boost::shared_ptr d_queue; @@ -127,18 +117,14 @@ private: int d_last_seg; long d_if_freq; long d_fs_in; - + int d_correlation_length_samples; + int d_n_correlator_taps; double d_early_late_spc_chips; double d_code_phase_step_chips; gr_complex* d_ca_code; - gr_complex* d_early_code; - gr_complex* d_late_code; - gr_complex* d_prompt_code; - gr_complex* d_carr_sign; - gr_complex *d_Early; gr_complex *d_Prompt; gr_complex *d_Late; @@ -148,15 +134,13 @@ private: double d_next_rem_code_phase_samples; float d_rem_carr_phase_rad; - // PLL and DLL filter library - Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; - // acquisition float d_acq_code_phase_samples; float d_acq_carrier_doppler_hz; // correlator - Correlator d_correlator; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; // tracking vars double d_code_freq_hz; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc index 828b4142e..b49149aa7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc @@ -126,20 +126,22 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( // Initialization of local code replica // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_malloc((GPS_L2_M_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment())); - - // Get space for the resampled early / prompt / late local replicas - 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())); - - // space for carrier wipeoff and signal baseband vectors - d_carr_sign = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); + d_ca_code = static_cast(volk_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment())); // correlator outputs (scalar) - 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_n_correlator_taps = 3; // Early, Prompt, and 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); + } + 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_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu.init(2 * d_vector_length, d_n_correlator_taps); //--- Perform initializations ------------------------------ @@ -237,9 +239,13 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() d_code_loop_filter.initialize(); // initialize the code filter // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l2c_m_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN); - d_ca_code[0] = d_ca_code[static_cast(GPS_L2_M_CODE_LENGTH_CHIPS)]; - d_ca_code[static_cast(GPS_L2_M_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1]; + gps_l2c_m_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L2_M_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; @@ -266,72 +272,16 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } - - - - -void gps_l2_m_dll_pll_tracking_cc::update_local_code() -{ - double tcode_chips; - double rem_code_phase_chips; - int associated_chip_index; - int code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); - double code_phase_step_chips; - int early_late_spc_samples; - int epl_loop_length_samples; - - // unified loop for E, P, L code vectors - code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); - tcode_chips = -rem_code_phase_chips; - - // Alternative EPL code generation (40% of speed improvement!) - early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); - epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2; - for (int i = 0; i < epl_loop_length_samples; i++) - { - associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); - d_early_code[i] = d_ca_code[associated_chip_index]; - tcode_chips = tcode_chips + code_phase_step_chips; - } - - memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex)); - memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex)); -} - - - - -void gps_l2_m_dll_pll_tracking_cc::update_local_carrier() -{ - float phase_rad, phase_step_rad; - - phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - phase_rad = d_rem_carr_phase_rad; - for(int i = 0; i < d_current_prn_length_samples; i++) - { - d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); - phase_rad += phase_step_rad; - } -} - - - - gps_l2_m_dll_pll_tracking_cc::~gps_l2_m_dll_pll_tracking_cc() { - d_dump_file.close(); + d_dump_file.close(); - volk_free(d_prompt_code); - volk_free(d_late_code); - volk_free(d_early_code); - volk_free(d_carr_sign); - volk_free(d_Early); - volk_free(d_Prompt); - volk_free(d_Late); - volk_free(d_ca_code); + volk_free(d_local_code_shift_chips); + volk_free(d_correlator_outs); + volk_free(d_ca_code); - delete[] d_Prompt_buffer; + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); } @@ -383,46 +333,18 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; - // 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 - d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_Early, - d_Prompt, - d_Late); - // check for samples consistency (this should be done before in the receiver / here only if the source is a file) - if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) - { - const int samples_available = ninput_items[0]; - d_sample_counter = d_sample_counter + samples_available; - LOG(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; - consume_each(samples_available); - - // make an output to not stop the rest of the processing blocks - current_synchro_data.Prompt_I = 0.0; - current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); - current_synchro_data.Carrier_phase_rads = 0.0; - current_synchro_data.Code_phase_secs = 0.0; - current_synchro_data.CN0_dB_hz = 0.0; - current_synchro_data.Flag_valid_tracking = false; - current_synchro_data.Flag_valid_pseudorange = false; - current_synchro_data.Flag_valid_symbol_output = false; - *out[0] = current_synchro_data; - - return 1; - } + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); // ################## PLL ########################################################## // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_L2_TWO_PI; + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_L2_TWO_PI; // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation @@ -437,7 +359,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int // ################## DLL ########################################################## // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti] + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator @@ -457,13 +379,23 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int T_prn_samples = T_prn_seconds * static_cast(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + + //################### PLL COMMANDS ################################################# + //carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + //################### DLL COMMANDS ################################################# + //code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES) { // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; d_cn0_estimation_counter++; } else @@ -496,8 +428,8 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int } } // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_I = static_cast(d_correlator_outs[1].real()); + current_synchro_data.Prompt_Q = static_cast(d_correlator_outs[1].imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in); @@ -512,7 +444,6 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_tracking = true; current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms=1; *out[0] = current_synchro_data; // ########## DEBUG OUTPUT @@ -562,9 +493,10 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int std::cout << tmp_str_stream.rdbuf() << std::flush; } } - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_symbol_output = false; @@ -573,16 +505,16 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int if(d_dump) { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - prompt_I = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); try { // EPR diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h index 8fdcfb689..5854f71ab 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h @@ -46,7 +46,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 gps_l2_m_dll_pll_tracking_cc; @@ -105,8 +105,6 @@ private: float pll_bw_hz, float dll_bw_hz, float early_late_space_chips); - void update_local_code(); - void update_local_carrier(); // tracking configuration vars boost::shared_ptr d_queue; @@ -122,19 +120,9 @@ private: double d_early_late_spc_chips; - gr_complex* d_ca_code; - - gr_complex* d_early_code; - gr_complex* d_late_code; - gr_complex* d_prompt_code; - gr_complex* d_carr_sign; - - gr_complex *d_Early; - gr_complex *d_Prompt; - gr_complex *d_Late; - // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; + double d_rem_code_phase_chips; double d_rem_carr_phase_rad; // PLL and DLL filter library @@ -145,11 +133,17 @@ private: double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // correlator - Correlator d_correlator; + int d_n_correlator_taps; + gr_complex* d_ca_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; // tracking vars double d_code_freq_chips; + double d_code_phase_step_chips; double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; double d_acc_code_phase_secs; diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index e113105e3..bc0dc85e3 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -31,7 +31,6 @@ endif(ENABLE_CUDA) set(TRACKING_LIB_SOURCES - correlator.cc cpu_multicorrelator.cc cpu_multicorrelator_16sc.cc lock_detectors.cc diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 2c179db53..93a4460c2 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -80,7 +80,6 @@ #include "galileo_e5a_noncoherent_iq_acquisition_caf.h" #include "gps_l1_ca_dll_pll_tracking.h" #include "gps_l1_ca_dll_pll_c_aid_tracking.h" -#include "gps_l1_ca_dll_fll_pll_tracking.h" #include "gps_l1_ca_tcp_connector_tracking.h" #include "galileo_e1_dll_pll_veml_tracking.h" #include "galileo_e1_tcp_connector_tracking.h" @@ -1323,12 +1322,6 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams, queue)); block = std::move(block_); } - else if (implementation.compare("GPS_L1_CA_DLL_FLL_PLL_Tracking") == 0) - { - std::unique_ptr block_(new GpsL1CaDllFllPllTracking(configuration.get(), role, in_streams, - out_streams, queue)); - block = std::move(block_); - } else if (implementation.compare("GPS_L1_CA_TCP_CONNECTOR_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaTcpConnectorTracking(configuration.get(), role, in_streams, @@ -1582,12 +1575,6 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams, queue)); block = std::move(block_); } - else if (implementation.compare("GPS_L1_CA_DLL_FLL_PLL_Tracking") == 0) - { - std::unique_ptr block_(new GpsL1CaDllFllPllTracking(configuration.get(), role, in_streams, - out_streams, queue)); - block = std::move(block_); - } else if (implementation.compare("GPS_L1_CA_TCP_CONNECTOR_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaTcpConnectorTracking(configuration.get(), role, in_streams,