diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index c1338bc1e..5b4b2c7fb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -36,9 +36,14 @@ include_directories( ${GFlags_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GNURADIO_RUNTIME_INCLUDE_DIRS} + ${VOLK_GNSSSDR_INCLUDE_DIRS} ) +if(ENABLE_GENERIC_ARCH) + add_definitions( -DGENERIC_ARCH=1 ) +endif(ENABLE_GENERIC_ARCH) + file(GLOB TRACKING_GR_BLOCKS_HEADERS "*.h") add_library(tracking_gr_blocks ${TRACKING_GR_BLOCKS_SOURCES} ${TRACKING_GR_BLOCKS_HEADERS}) source_group(Headers FILES ${TRACKING_GR_BLOCKS_HEADERS}) -target_link_libraries(tracking_gr_blocks tracking_lib ${GNURADIO_RUNTIME_LIBRARIES} gnss_sp_libs ${Boost_LIBRARIES} ) +target_link_libraries(tracking_gr_blocks tracking_lib ${GNURADIO_RUNTIME_LIBRARIES} gnss_sp_libs ${Boost_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ) 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 a04dc59fc..5ae4fe191 100644 --- 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 @@ -7,11 +7,11 @@ * Code DLL + carrier PLL according to the algorithms described in: * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkha user, 2007 + * Approach, Birkhauser, 2007 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -21,7 +21,7 @@ * 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. + * (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 @@ -37,6 +37,7 @@ #include "galileo_e1_dll_pll_veml_tracking_cc.h" #include #include +#include #include #include #include @@ -47,6 +48,7 @@ #include "lock_detectors.h" #include "Galileo_E1.h" #include "control_message_factory.h" +#include "volk_gnsssdr/volk_gnsssdr.h" @@ -130,25 +132,26 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( * to performance degradation. Here we allocate memory * (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes */ - // todo: do something if posix_memalign fails - // Get space for the resampled early / prompt / late local replicas - if (posix_memalign((void**)&d_very_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; - if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; - if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; - if (posix_memalign((void**)&d_late_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; - if (posix_memalign((void**)&d_very_late_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; - // space for carrier wipeoff and signal baseband vectors - if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; + + d_very_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + d_very_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + + d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); + // correlator outputs (scalar) - if (posix_memalign((void**)&d_Very_Early, 16, sizeof(gr_complex)) == 0){}; - if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){}; - if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){}; - if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; - if (posix_memalign((void**)&d_Very_Late, 16, sizeof(gr_complex)) == 0){}; + + d_Very_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); + d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); + d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); + d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); + d_Very_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); //--- Initializations ------------------------------ // Initial code frequency basis of NCO - d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_freq_chips = (double)Galileo_E1_CODE_CHIP_RATE_HZ; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; // Residual carrier phase @@ -283,17 +286,17 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { d_dump_file.close(); - free(d_very_early_code); - free(d_early_code); - free(d_prompt_code); - free(d_late_code); - free(d_very_late_code); - free(d_carr_sign); - free(d_Very_Early); - free(d_Early); - free(d_Prompt); - free(d_Late); - free(d_Very_Late); + 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); delete[] d_ca_code; delete[] d_Prompt_buffer; @@ -342,20 +345,22 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect update_local_carrier(); // perform carrier wipe-off and compute Very Early, Early, Prompt, Late and Very Late correlation - d_correlator.Carrier_wipeoff_and_VEPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_very_early_code, - d_early_code, - d_prompt_code, - d_late_code, - d_very_late_code, - d_Very_Early, - d_Early, - d_Prompt, - d_Late, - d_Very_Late, - is_unaligned()); +// 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, +// is_unaligned()); + + volk_gnsssdr_32fc_x7_cw_vepl_corr_32fc_x5(d_Very_Early, d_Early, d_Prompt, d_Late, d_Very_Late, in, d_carr_sign, d_very_early_code, d_early_code, d_prompt_code, d_late_code, d_very_late_code, d_current_prn_length_samples); // ################## PLL ########################################################## // PLL discriminator @@ -369,7 +374,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect //carrier phase accumulator for (K) Doppler estimation d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI* d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); // ################## DLL ########################################################## @@ -385,17 +390,17 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / d_code_freq_chips; + T_chip_seconds = 1 / (double)d_code_freq_chips; T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * (float)d_fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)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 + //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) @@ -427,12 +432,11 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -442,9 +446,18 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); - // Tracking_timestamp_secs is aligned with the PRN start sample - current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + - (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; + + // Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!) + //compute remnant code phase samples BEFORE the Tracking timestamp + //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + + // (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; + + // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) + current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples) / (double)d_fs_in; + //compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; @@ -558,6 +571,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect } consume_each(d_current_prn_length_samples); // this is required for gr_block derivates d_sample_counter += d_current_prn_length_samples; //count for the processed samples + //std::cout<<"Galileo tracking output at sample "<