diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 8df2646f5..a02b9fc59 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -159,6 +159,8 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] // update the pseudorange object current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].debug_var1=delta_rx_time_ms; + //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads+ GPS_TWO_PI*0.001*delta_rx_time_ms*current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GPS_STARTOFFSET_ms/1000.0; @@ -175,11 +177,16 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni { tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; + //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].Pseudorange_m; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true); + //tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true); + //tmp_double = current_gnss_synchro[i].debug_var1; + tmp_double= current_gnss_synchro[i].debug_var2; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = current_gnss_synchro[i].PRN; d_dump_file.write((char*)&tmp_double, sizeof(double)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc index 292747cd3..f84c3448d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc @@ -35,7 +35,7 @@ #include #include #include -#include // fixed point sine and cosine +#include #include #include "gnss_synchro.h" #include "gps_sdr_signal_processing.h" @@ -102,6 +102,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( d_fs_in = fs_in; d_vector_length = vector_length; d_dump_filename = dump_filename; + d_current_prn_length_samples = static_cast(d_vector_length); // Initialize tracking ========================================== d_code_loop_filter.set_DLL_BW(dll_bw_hz); @@ -112,21 +113,22 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_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(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_current_prn_length_samples,d_n_correlator_taps); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -134,7 +136,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( // define residual code phase (in chips) d_rem_code_phase_samples = 0.0; // define residual carrier phase - d_rem_carr_phase_rad = 0.0; + d_rem_carrier_phase_rad = 0.0; // sample synchronization d_sample_counter = 0; @@ -145,8 +147,6 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( d_pull_in = false; d_last_seg = 0; - d_current_prn_length_samples = static_cast(d_vector_length); - // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; @@ -170,13 +170,14 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( d_acc_carrier_phase_rad = 0.0; d_code_phase_samples = 0.0; - d_pll_to_dll_assist_secs_ti=0.0; + d_pll_to_dll_assist_secs_Ti=0.0; //set_min_output_buffer((long int)300); } void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking() { + /* * correct the code phase according to the delay between acq and trk */ @@ -197,6 +198,7 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking() float T_prn_mod_seconds; float T_prn_mod_samples; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); @@ -218,20 +220,28 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad=GPS_TWO_PI*d_carrier_doppler_hz/static_cast(d_fs_in); // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); + d_carrier_loop_filter.initialize(d_acq_carrier_doppler_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) - 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;nwhat(); } } @@ -632,9 +565,9 @@ void gps_l1_ca_dll_pll_artemisa_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; + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl; } } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h index b3ddf8027..26ed6a070 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h @@ -50,7 +50,7 @@ #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_FLL_PLL_filter.h" -#include "correlator.h" +#include "cpu_multicorrelator.h" class gps_l1_ca_dll_pll_artemisa_tracking_cc; @@ -109,8 +109,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; @@ -125,21 +123,17 @@ private: long d_fs_in; double d_early_late_spc_chips; + int d_n_correlator_taps; 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; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + float d_rem_code_phase_chips; + float d_rem_carrier_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; @@ -148,15 +142,15 @@ private: // acquisition float d_acq_code_phase_samples; float d_acq_carrier_doppler_hz; - // correlator - Correlator d_correlator; // tracking vars double d_code_freq_chips; + float d_code_phase_step_chips; float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; + float d_carrier_phase_step_rad; + double d_acc_carrier_phase_rad; float d_code_phase_samples; - float d_pll_to_dll_assist_secs_ti; + float d_pll_to_dll_assist_secs_Ti; //PRN period in samples int d_current_prn_length_samples; diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 7d0fb9d07..cc53a9184 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -38,6 +38,7 @@ endif(ENABLE_CUDA) set(TRACKING_LIB_SOURCES correlator.cc + cpu_multicorrelator.cc lock_detectors.cc tcp_communication.cc tcp_packet_data.cc diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.cc b/src/algorithms/tracking/libs/cpu_multicorrelator.cc new file mode 100644 index 000000000..0ed2ac9d8 --- /dev/null +++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc @@ -0,0 +1,167 @@ +/*! + * \file cpu_multicorrelator.cc + * \brief High optimized CPU vector multiTAP correlator class + * \authors
    + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * Class that implements a high optimized vector multiTAP correlator class for CPUs + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "cpu_multicorrelator.h" + +#include +#include +#include // fixed point sine and cosine +#include + +bool cpu_multicorrelator::init( + int max_signal_length_samples, + int n_correlators + ) +{ + + // ALLOCATE MEMORY FOR INTERNAL vectors + size_t size = max_signal_length_samples * sizeof(std::complex); + + // NCO signal + d_nco_in=static_cast*>(volk_malloc(size, volk_get_alignment())); + + // Doppler-free signal + d_sig_doppler_wiped=static_cast*>(volk_malloc(size, volk_get_alignment())); + + d_local_codes_resampled=new std::complex*[n_correlators]; + for (int n=0;n*>(volk_malloc(size, volk_get_alignment())); + } + d_n_correlators=n_correlators; + return true; +} + +bool cpu_multicorrelator::set_local_code_and_taps( + int code_length_chips, + const std::complex* local_code_in, + float *shifts_chips + ) +{ + + d_local_code_in=local_code_in; + d_shifts_chips=shifts_chips; + d_code_length_chips=code_length_chips; + return true; +} + +bool cpu_multicorrelator::set_input_output_vectors( + std::complex* corr_out, + const std::complex* sig_in + ) +{ + // Save CPU pointers + d_sig_in =sig_in; + d_corr_out = corr_out; + return true; + +} + +void cpu_multicorrelator::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips) +{ + float local_code_chip_index; + for (int current_correlator_tap=0; current_correlator_tap(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips); + //Take into account that in multitap correlators, the shifts can be negative! + if (local_code_chip_index<0.0) local_code_chip_index+=d_code_length_chips; + d_local_codes_resampled[current_correlator_tap][n]=d_local_code_in[static_cast(round(local_code_chip_index))]; + + } + } +} + + +void cpu_multicorrelator::update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad) +{ + float sin_f, cos_f; + int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad); + int phase_rad_i = gr::fxpt::float_to_fixed(rem_carr_phase_rad); + + for(int i = 0; i < correlator_length_samples; i++) + { + gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f); + d_nco_in[i] = std::complex(cos_f, -sin_f); + phase_rad_i += phase_step_rad_i; + } +} + +bool cpu_multicorrelator::Carrier_wipeoff_multicorrelator_resampler( + float rem_carrier_phase_in_rad, + float phase_step_rad, + float rem_code_phase_chips, + float code_phase_step_chips, + int signal_length_samples) + { + + update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad); + update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips); + + volk_32fc_x2_multiply_32fc(d_sig_doppler_wiped, d_sig_in, d_nco_in, signal_length_samples); + for (int current_correlator_tap=0; current_correlator_tap + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + * + * + * Class that implements a high optimized vector multiTAP correlator class for CPUs + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef CPU_MULTICORRELATOR_H_ +#define CPU_MULTICORRELATOR_H_ + +#include + +/*! + * \brief Class that implements carrier wipe-off and correlators. + */ +class cpu_multicorrelator +{ +public: + cpu_multicorrelator(); + bool init( + int max_signal_length_samples, + int n_correlators + ); + bool set_local_code_and_taps( + int code_length_chips, + const std::complex* local_code_in, + float *shifts_chips + ); + bool set_input_output_vectors( + std::complex* corr_out, + const std::complex* sig_in + ); + void update_local_code( + int correlator_length_samples, + float rem_code_phase_chips, + float code_phase_step_chips + ); + + void update_local_carrier( + int correlator_length_samples, + float rem_carr_phase_rad, + float phase_step_rad + ); + bool Carrier_wipeoff_multicorrelator_resampler( + float rem_carrier_phase_in_rad, + float phase_step_rad, + float rem_code_phase_chips, + float code_phase_step_chips, + int signal_length_samples); + bool free(); + +private: + // Allocate the device input vectors + const std::complex *d_sig_in; + std::complex *d_nco_in; + std::complex **d_local_codes_resampled; + std::complex *d_sig_doppler_wiped; + const std::complex *d_local_code_in; + std::complex *d_corr_out; + + float *d_shifts_chips; + int d_code_length_chips; + int d_n_correlators; + + bool update_local_code(); + bool update_local_carrier(); + +}; + + +#endif /* CPU_MULTICORRELATOR_H_ */ diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index df688e178..600f12285 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -73,6 +73,10 @@ public: // Pseudorange double Pseudorange_m; bool Flag_valid_pseudorange; + + //debug + double debug_var1; + double debug_var2; }; #endif