diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc deleted file mode 100644 index 265b3eedb..000000000 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc +++ /dev/null @@ -1,163 +0,0 @@ -/*! - * \file gps_l1_ca_dll_fll_pll_tracking.cc - * \brief Implementation of an adapter of a code DLL + carrier FLL/PLL tracking - * loop for GPS L1 C/A to a TrackingInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * This file implements the code Delay Locked Loop (DLL) + carrier Phase - * Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) - * according to the algorithms described in: - * E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and - * Applications, Second Edition, Artech House Publishers, 2005. - * - * ------------------------------------------------------------------------- - * - * 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 "gps_l1_ca_dll_fll_pll_tracking.h" -#include -#include "GPS_L1_CA.h" -#include "configuration_interface.h" - - -using google::LogMessage; - -GpsL1CaDllFllPllTracking::GpsL1CaDllFllPllTracking( - ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, unsigned int - out_streams, - boost::shared_ptr queue) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams), - queue_(queue) -{ - DLOG(INFO) << "role " << role; - //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; - std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float fll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - int order; - item_type = configuration->property(role + ".item_type",default_item_type); - //vector_length = configuration->property(role + ".vector_length", 2048); - fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - order = configuration->property(role + ".order", 2); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - fll_bw_hz = configuration->property(role + ".fll_bw_hz", 100.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); - std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); - - //################# MAKE TRACKING GNURadio object ################### - if (item_type.compare("gr_complex") == 0) - { - item_size_ = sizeof(gr_complex); - tracking_ = gps_l1_ca_dll_fll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - queue_, - dump, - dump_filename, - order, - fll_bw_hz, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); - } - else - { - item_size_ = sizeof(gr_complex); - LOG(WARNING) << item_type << " unknown tracking item type."; - } - channel_ = 0; - channel_internal_queue_ = 0; - - DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; -} - - -GpsL1CaDllFllPllTracking::~GpsL1CaDllFllPllTracking() -{} - - -void GpsL1CaDllFllPllTracking::start_tracking() -{ - tracking_->start_tracking(); -} - -void GpsL1CaDllFllPllTracking::set_channel(unsigned int channel) -{ - channel_ = channel; - tracking_->set_channel(channel); -} - -void GpsL1CaDllFllPllTracking::set_channel_queue( - concurrent_queue *channel_internal_queue) -{ - channel_internal_queue_ = channel_internal_queue; - tracking_->set_channel_queue(channel_internal_queue_); -} - -void GpsL1CaDllFllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) -{ - return tracking_->set_gnss_synchro(p_gnss_synchro); -} - -void GpsL1CaDllFllPllTracking::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - //nothing to connect, now the tracking uses gr_sync_decimator -} - -void GpsL1CaDllFllPllTracking::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - //nothing to disconnect, now the tracking uses gr_sync_decimator -} - -gr::basic_block_sptr GpsL1CaDllFllPllTracking::get_left_block() -{ - return tracking_; -} - -gr::basic_block_sptr GpsL1CaDllFllPllTracking::get_right_block() -{ - return tracking_; -} - diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h deleted file mode 100644 index 7fd84a9d7..000000000 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h +++ /dev/null @@ -1,99 +0,0 @@ -/*! - * \file gps_l1_ca_dll_fll_pll_tracking.h - * \brief Interface of an adapter of a code DLL + carrier FLL/PLL tracking - * loop for GPS L1 C/A to a TrackingInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * This is the interface of a code Delay Locked Loop (DLL) + carrier - * Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) - * according to the algorithms described in: - * E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and - * Applications, Second Edition, Artech House Publishers, 2005. - * - * ------------------------------------------------------------------------- - * - * 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 GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_ -#define GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_ - -#include -#include -#include "tracking_interface.h" -#include "gps_l1_ca_dll_fll_pll_tracking_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements a code DLL + carrier PLL/FLL Assisted tracking loop - */ -class GpsL1CaDllFllPllTracking : public TrackingInterface -{ -public: - GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams, - boost::shared_ptr queue); - - virtual ~GpsL1CaDllFllPllTracking(); - - std::string role() - { - return role_; - } - - //! Returns "GPS_L1_CA_DLL_FLL_PLL_Tracking" - std::string implementation() - { - return "GPS_L1_CA_DLL_FLL_PLL_Tracking"; - } - size_t item_size() - { - return item_size_; - } - - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - - void set_channel(unsigned int channel); - void set_channel_queue(concurrent_queue *channel_internal_queue); - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); - void start_tracking(); - -private: - gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_; - size_t item_size_; - unsigned int channel_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; - boost::shared_ptr queue_; - concurrent_queue *channel_internal_queue_; -}; - -#endif // GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc deleted file mode 100644 index 7d8216886..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc +++ /dev/null @@ -1,690 +0,0 @@ -/*! - * \file gps_l1_ca_dll_fll_pll_tracking_cc.cc - * \brief Implementation of a code DLL + carrier FLL/PLL tracking block - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * This file implements the code Delay Locked Loop (DLL) + carrier - * Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) - * according to the algorithms described in: - * E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and - * Applications, Second Edition, Artech House Publishers, 2005. - * - * ------------------------------------------------------------------------- - * - * 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 "gps_l1_ca_dll_fll_pll_tracking_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include "gps_sdr_signal_processing.h" -#include "GPS_L1_CA.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "control_message_factory.h" - - -/*! - * \todo Include in definition header file - */ -#define CN0_ESTIMATION_SAMPLES 20 -#define MINIMUM_VALID_CN0 25 -#define MAXIMUM_LOCK_FAIL_COUNTER 50 -#define CARRIER_LOCK_THRESHOLD 0.85 - -using google::LogMessage; - -gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc( - long if_freq, - long fs_in, - unsigned - int vector_length, - boost::shared_ptr queue, - bool dump, std::string dump_filename, - int order, - float fll_bw_hz, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) -{ - return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(if_freq, - fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz, - early_late_space_chips)); -} - - - - -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - ninput_items_required[0] = d_vector_length * 2; //set the required available samples in each call - } -} - - -Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - boost::shared_ptr queue, - bool dump, - std::string dump_filename, - int order, - float fll_bw_hz, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) : - gr::block("Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) -{ - // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); - // initialize internal vars - d_queue = queue; - d_dump = dump; - - d_acquisition_gnss_synchro = NULL; - - d_if_freq = static_cast(if_freq); - d_fs_in = static_cast(fs_in); - d_vector_length = vector_length; - d_early_late_spc_chips = static_cast(early_late_space_chips); // Define early-late offset (in chips) - d_dump_filename = dump_filename; - - // Initialize tracking variables ========================================== - d_carrier_loop_filter.set_params(fll_bw_hz, pll_bw_hz,order); - - d_code_loop_filter = Tracking_2nd_DLL_filter(GPS_L1_CA_CODE_PERIOD); - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - - // 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())); - - // 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())); - - // sample synchronization - d_sample_counter = 0; - d_acq_sample_stamp = 0; - d_last_seg = 0;// this is for debug output only - d_code_phase_samples = 0; - d_enable_tracking = false; - 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]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; - - systemName["G"] = std::string("GPS"); - systemName["R"] = std::string("GLONASS"); - systemName["S"] = std::string("SBAS"); - systemName["E"] = std::string("Galileo"); - systemName["C"] = std::string("Compass"); - - d_channel_internal_queue = 0; - //d_acquisition_gnss_synchro = 0; - d_channel = 0; - d_acq_carrier_doppler_hz = 0.0; - d_carrier_doppler_hz = 0; - d_code_freq_hz = 0.0; - d_rem_carr_phase = 0.0; - d_rem_code_phase_samples = 0.0; - d_acq_code_phase_samples = 0; - d_acq_carrier_doppler_hz = 0.0; - d_acc_carrier_phase_rad = 0.0; - d_acc_code_phase_samples = 0; - d_FLL_discriminator_hz = 0.0; - d_pull_in = false; - d_FLL_wait = 1; -} - - - - -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking() -{ - /* - * correct the code phase according to the delay between acq and trk - */ - d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; - d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; - d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - - long int acq_trk_diff_samples; - float acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; - //doppler effect - // Fd=(C/(C+Vr))*F - double radial_velocity; - radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; - // new chip and prn sequence periods based on acq Doppler - double T_chip_mod_seconds; - double T_prn_mod_seconds; - double T_prn_mod_samples; - d_code_freq_hz = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; - T_chip_mod_seconds = 1 / d_code_freq_hz; - T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * d_fs_in; - d_current_prn_length_samples = round(T_prn_mod_samples); - - double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * d_fs_in; - double T_prn_diff_seconds; - T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; - double N_prn_diff; - N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - double corrected_acq_phase_samples, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in), T_prn_true_samples); - - if (corrected_acq_phase_samples < 0) - { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; - } - 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(d_acq_carrier_doppler_hz); - d_FLL_wait = 1; - - // 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]; - - d_carrier_lock_fail_counter = 0; - d_Prompt_prev = 0; - d_rem_code_phase_samples = 0; - d_rem_carr_phase = 0; - d_FLL_discriminator_hz = 0; - d_rem_code_phase_samples = 0; - d_acc_carrier_phase_rad = 0; - - std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0,1); - - // DEBUG OUTPUT - std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking Gnss_Satellite(systemName[&d_acquisition_gnss_synchro->System], d_acquisition_gnss_synchro->PRN) - d_pull_in = true; - d_enable_tracking = true; - - 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::endl; -} - - - - - -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code() -{ - double tcode_chips; - double rem_code_phase_chips; - double code_phase_step_chips; - int early_late_spc_samples; - int epl_loop_length_samples; - - int associated_chip_index; - int code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); - code_phase_step_chips = d_code_freq_hz / d_fs_in; - rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in); - // unified loop for E, P, L code vectors - 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_Fll_Pll_Tracking_cc::update_local_carrier() -{ - double phase, phase_step; - phase_step = GPS_TWO_PI * d_carrier_doppler_hz / d_fs_in; - phase = d_rem_carr_phase; - for(int i = 0; i < d_current_prn_length_samples; i++) - { - d_carr_sign[i] = gr_complex(cos(phase), -sin(phase)); - phase += phase_step; - } - d_rem_carr_phase = fmod(phase, GPS_TWO_PI); - d_acc_carrier_phase_rad -= d_acc_carrier_phase_rad + phase; -} - - - -Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc() -{ - d_dump_file.close(); - - volk_free(d_ca_code); - 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); - - delete[] d_Prompt_buffer; -} - - - -int Gps_L1_Ca_Dll_Fll_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) -{ - double code_error_chips = 0; - double code_error_filt_chips = 0; - double correlation_time_s = 0; - double PLL_discriminator_hz = 0; - double carr_nco_hz = 0; - // get the sample in and out pointers - const gr_complex* in = (gr_complex*) input_items[0]; // block input samples pointer - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // block output streams pointer - - d_Prompt_prev = *d_Prompt; // for the FLL discriminator - - if (d_enable_tracking == true) - { - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data; - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - /* - * Receiver signal alignment - */ - if (d_pull_in == true) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - // /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE - d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples - d_pull_in = false; - consume_each(samples_offset); //shift input to perform alignment with local replica - - // 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) / 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; - } - - update_local_code(); - update_local_carrier(); - - // 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); - // 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) / 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; - } - - /* - * DLL, FLL, and PLL discriminators - */ - // Compute DLL error - code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); - // Compute DLL filtered error - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); - - //compute FLL error - correlation_time_s = (static_cast(d_current_prn_length_samples)) / d_fs_in; - if (d_FLL_wait == 1) - { - d_Prompt_prev = *d_Prompt; - d_FLL_discriminator_hz = 0.0; - d_FLL_wait = 0; - } - else - { - d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, *d_Prompt, 0, correlation_time_s) / GPS_TWO_PI; - d_Prompt_prev = *d_Prompt; - d_FLL_wait = 1; - } - - // Compute PLL error - PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI; - - /* - * DLL and FLL+PLL filter and get current carrier Doppler and code frequency - */ - carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s); - d_carrier_doppler_hz = d_if_freq + carr_nco_hz; - - d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ + (((d_carrier_doppler_hz + d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); - - /*! - * \todo Improve the lock detection algorithm! - */ - // ####### 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_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - //d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in); - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); - // ###### TRACKING UNLOCK NOTIFICATION ##### - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - std::unique_ptr cmf(new ControlMessageFactory()); - if (d_queue != gr::msg_queue::sptr()) - { - d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); - } - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - - // ########## DEBUG OUTPUT - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // debug: Second counter in channel 0 - if (d_channel == 0) - { - if (floor(d_sample_counter/d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); - std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; - LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; - } - } - else - { - if (floor(d_sample_counter/d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter/d_fs_in); - LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; - } - } - - //predict the next loop PRN period length prediction - double T_chip_seconds; - double T_prn_seconds; - double T_prn_samples; - double K_blk_samples; - T_chip_seconds = 1 / static_cast(d_code_freq_hz); - T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * d_fs_in; - - float code_error_filt_samples; - code_error_filt_samples = GPS_L1_CA_CODE_PERIOD * code_error_filt_chips * GPS_L1_CA_CHIP_PERIOD * static_cast(d_fs_in); //[seconds] - d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples; - - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples; - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete sample - //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error - - // ########### 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()); - // 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; - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples))/static_cast(d_fs_in); - 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, Code_phase_secs=0 - current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - 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; - current_synchro_data.Flag_valid_pseudorange = false; - *out[0] = current_synchro_data; - } - else - { - // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // stream to collect cout calls to improve thread safety - std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); - - if (d_channel == 0) - { - // debug: Second counter in channel 0 - tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; - 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); - - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer - d_acquisition_gnss_synchro->Flag_valid_pseudorange = false; - d_acquisition_gnss_synchro->Flag_valid_symbol_output = false; - *out[0] = *d_acquisition_gnss_synchro; - } - - - if(d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - float tmp_float; - 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); - try - { - // EPR - d_dump_file.write((char*)&tmp_E, sizeof(float)); - d_dump_file.write((char*)&tmp_P, sizeof(float)); - d_dump_file.write((char*)&tmp_L, sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write((char*)&prompt_I, sizeof(float)); - d_dump_file.write((char*)&prompt_Q, sizeof(float)); - // PRN start sample stamp - d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); - // accumulated carrier phase - tmp_float = (float)d_acc_carrier_phase_rad; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - - // carrier and code frequency - tmp_float = (float)d_carrier_doppler_hz; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - tmp_float = (float)d_code_freq_hz; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - - //PLL commands - tmp_float = (float)PLL_discriminator_hz; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - tmp_float = (float)carr_nco_hz; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - - //DLL commands - tmp_float = (float)code_error_chips; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - tmp_float = (float)code_error_filt_chips; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - - // CN0 and carrier lock test - tmp_float = (float)d_CN0_SNV_dB_Hz; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - tmp_float = (float)d_carrier_lock_test; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - - // AUX vars (for debug purposes) - tmp_float = (float)d_rem_code_phase_samples; - d_dump_file.write((char*)&tmp_float, sizeof(float)); - 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) - { - LOG(INFO) << "Exception writing trk dump file "<< e.what() << std::endl; - } - } - 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"; - } - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false -} - - -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_channel(unsigned int channel) -{ - d_channel = channel; - LOG(INFO) << "Tracking Channel set to " << d_channel; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(d_channel)); - d_dump_filename.append(".dat"); - d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); - 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) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } - } -} - - - -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_channel_queue(concurrent_queue *channel_internal_queue) -{ - d_channel_internal_queue = channel_internal_queue; -} - -void Gps_L1_Ca_Dll_Fll_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_fll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h deleted file mode 100644 index 3213821cf..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h +++ /dev/null @@ -1,206 +0,0 @@ -/*! - * \file gps_l1_ca_dll_fll_pll_tracking_cc.h - * \brief Interface of a code DLL + carrier FLL/PLL tracking block - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * This is the interface of a code Delay Locked Loop (DLL) + - * carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked - * Loop (FLL) according to the algorithms described in: - * E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and - * Applications, Second Edition, Artech House Publishers, 2005. - * - * ------------------------------------------------------------------------- - * - * 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 GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H -#define GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H - -#include -#include -#include -#include -#include -#include "concurrent_queue.h" -#include "tracking_FLL_PLL_filter.h" -#include "tracking_2nd_DLL_filter.h" -#include "gnss_synchro.h" -#include "correlator.h" - -class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc; - -typedef boost::shared_ptr -gps_l1_ca_dll_fll_pll_tracking_cc_sptr; - -gps_l1_ca_dll_fll_pll_tracking_cc_sptr -gps_l1_ca_dll_fll_pll_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - boost::shared_ptr queue, - bool dump, - std::string dump_filename, - int order, - float fll_bw_hz, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - -/*! - * \brief This class implements a DLL and a FLL assisted PLL tracking loop block - */ -class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc: public gr::block -{ -public: - ~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(); - - void set_channel(unsigned int channel); - void start_tracking(); - void update_local_code(); - void update_local_carrier(); - void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz); - /* - * \brief Satellite signal synchronization parameters uses shared memory between acquisition and tracking - */ - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); - void set_channel_queue(concurrent_queue *channel_internal_queue); - - /* - * \brief just like gr_block::general_work, only this arranges to call consume_each for you - * - * The user must override work to define the signal processing code - */ - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - void forecast (int noutput_items, gr_vector_int &ninput_items_required); -private: - friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr - gps_l1_ca_dll_fll_pll_make_tracking_cc( - long if_freq, - long fs_in, unsigned - int vector_length, - boost::shared_ptr queue, - bool dump, - std::string dump_filename, - int order, - float fll_bw_hz, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc( - long if_freq, - long fs_in, unsigned - int vector_length, - boost::shared_ptr queue, - bool dump, - std::string dump_filename, - int order, - float fll_bw_hz, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - void CN0_estimation_and_lock_detectors(); - - // class private vars - Gnss_Synchro* d_acquisition_gnss_synchro; - boost::shared_ptr d_queue; - concurrent_queue *d_channel_internal_queue; - unsigned int d_vector_length; - bool d_dump; - unsigned int d_channel; - int d_last_seg; - double d_if_freq; - double d_fs_in; - - 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; - - gr_complex d_Prompt_prev; - - double d_early_late_spc_chips; - - double d_carrier_doppler_hz; - double d_code_freq_hz; - double d_code_phase_samples; - int d_current_prn_length_samples; - //int d_next_prn_length_samples; - int d_FLL_wait; - double d_rem_carr_phase; - double d_rem_code_phase_samples; - //double d_next_rem_code_phase_samples; - bool d_pull_in; - - // acquisition - double d_acq_code_phase_samples; - double d_acq_carrier_doppler_hz; - - // correlator - Correlator d_correlator; - - // FLL + PLL filter - double d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory - Tracking_FLL_PLL_filter d_carrier_loop_filter; - double d_acc_carrier_phase_rad; - double d_acc_code_phase_samples; - - Tracking_2nd_DLL_filter d_code_loop_filter; - - unsigned long int d_sample_counter; - - unsigned long int d_acq_sample_stamp; - - // CN0 estimation and lock detector - int d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; - double d_carrier_lock_test; - double d_CN0_SNV_dB_Hz; - - double d_carrier_lock_threshold; - - int d_carrier_lock_fail_counter; - - bool d_enable_tracking; - - std::string d_dump_filename; - std::ofstream d_dump_file; - - std::map systemName; - std::string sys; -}; - -#endif //GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H diff --git a/src/algorithms/tracking/libs/correlator.cc b/src/algorithms/tracking/libs/correlator.cc deleted file mode 100644 index 967bb5c40..000000000 --- a/src/algorithms/tracking/libs/correlator.cc +++ /dev/null @@ -1,201 +0,0 @@ -/*! - * \file correlator.cc - * \brief Highly optimized vector correlator class - * \authors
    - *
  • Javier Arribas, 2011. jarribas(at)cttc.es - *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com - *
- * - * Class that implements a high optimized vector correlator class. - * - * ------------------------------------------------------------------------- - * - * 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 "correlator.h" -#include - -#if USING_VOLK_CW_EPL_CORR_CUSTOM - #define LV_HAVE_SSE3 - #include "volk_cw_epl_corr.h" -#endif - - -unsigned long Correlator::next_power_2(unsigned long v) -{ - v--; - v |= v >> 1; - v |= v >> 2; - v |= v >> 4; - v |= v >> 8; - v |= v >> 16; - v++; - return v; -} - - - - -void Correlator::Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code,gr_complex* E_out, gr_complex* P_out, gr_complex* L_out) -{ - gr_complex bb_signal_sample(0,0); - - *E_out = 0; - *P_out = 0; - *L_out = 0; - // perform Early, Prompt and Late correlation - for(int i=0; i < signal_length_samples; ++i) - { - //Perform the carrier wipe-off - bb_signal_sample = input[i] * carrier[i]; - // Now get early, late, and prompt values for each - *E_out += bb_signal_sample * E_code[i]; - *P_out += bb_signal_sample * P_code[i]; - *L_out += bb_signal_sample * L_code[i]; - } -} - - - - -void Correlator::Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out) -{ - gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); - - volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples); - - volk_free(bb_signal); -} - -//void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out) -//{ -// gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); -// volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, integration_time * prn_length_samples); -// volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, integration_time * prn_length_samples); -// volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, integration_time * prn_length_samples); -// volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, integration_time * prn_length_samples); -// // Vector of Prompts of I code -// for (int i = 0; i < integration_time; i++) -// { -// volk_32fc_x2_dot_prod_32fc(&P_data_out[i], &bb_signal[i*prn_length_samples], P_data_code, prn_length_samples); -// } -// -// volk_free(bb_signal); -//} - -void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out) -{ - gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); - - volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_data_out, bb_signal, P_data_code, signal_length_samples); - - volk_free(bb_signal); -} - - -void Correlator::Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out) -{ - gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); - - volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(VE_out, bb_signal, VE_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(VL_out, bb_signal, VL_code, signal_length_samples); - - volk_free(bb_signal); -} - - - -Correlator::Correlator () -{} - - -Correlator::~Correlator () -{} - - -#if USING_VOLK_CW_EPL_CORR_CUSTOM -void Correlator::Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier,gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out) -{ - volk_cw_epl_corr_u(input, carrier, E_code, P_code, L_code, E_out, P_out, L_out, signal_length_samples); -} -#endif -void Correlator::Carrier_rotate_and_EPL_volk(int signal_length_samples, - const gr_complex* input, - gr_complex *phase_as_complex, - gr_complex phase_inc_as_complex, - const gr_complex* E_code, - const gr_complex* P_code, - const gr_complex* L_code, - gr_complex* E_out, - gr_complex* P_out, - gr_complex* L_out ) -{ - gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); - - volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples); - - volk_free(bb_signal); -} - -void Correlator::Carrier_rotate_and_VEPL_volk(int signal_length_samples, - const gr_complex* input, - gr_complex *phase_as_complex, - gr_complex phase_inc_as_complex, - const gr_complex* VE_code, - const gr_complex* E_code, - const gr_complex* P_code, - const gr_complex* L_code, - const gr_complex* VL_code, - gr_complex* VE_out, - gr_complex* E_out, - gr_complex* P_out, - gr_complex* L_out, - gr_complex* VL_out ) -{ - gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); - - volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(VE_out, bb_signal, VE_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples); - volk_32fc_x2_dot_prod_32fc(VL_out, bb_signal, VL_code, signal_length_samples); - - volk_free(bb_signal); -} diff --git a/src/algorithms/tracking/libs/correlator.h b/src/algorithms/tracking/libs/correlator.h deleted file mode 100644 index ea8672846..000000000 --- a/src/algorithms/tracking/libs/correlator.h +++ /dev/null @@ -1,99 +0,0 @@ -/*! - * \file correlator.h - * \brief High optimized vector correlator class - * \authors
    - *
  • Javier Arribas, 2011. jarribas(at)cttc.es - *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com - *
- * - * Class that implements a high optimized vector correlator class - * using the volk library - * - * ------------------------------------------------------------------------- - * - * 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 GNSS_SDR_CORRELATOR_H_ -#define GNSS_SDR_CORRELATOR_H_ - -#include - -#if !defined(GENERIC_ARCH) && HAVE_SSE3 -#define USING_VOLK_CW_EPL_CORR_CUSTOM 1 -#endif - -/*! - * \brief Class that implements carrier wipe-off and correlators. - * - * Implemented versions: - * - Generic: Standard C++ implementation. - * - Volk: uses VOLK (Vector-Optimized Library of Kernels) and uses the processor's SIMD instruction sets. See http://gnuradio.org/redmine/projects/gnuradio/wiki/Volk - * - */ -class Correlator -{ -public: - Correlator(); - ~Correlator(); - void Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out); - void Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out); - void Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out); - // void Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out); - void Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out); - - void Carrier_rotate_and_EPL_volk(int signal_length_samples, - const gr_complex* input, - gr_complex *phase_as_complex, - gr_complex phase_inc_as_complex, - const gr_complex* E_code, - const gr_complex* P_code, - const gr_complex* L_code, - gr_complex* E_out, - gr_complex* P_out, - gr_complex* L_out ); - - void Carrier_rotate_and_VEPL_volk(int signal_length_samples, - const gr_complex* input, - gr_complex *phase_as_complex, - gr_complex phase_inc_as_complex, - const gr_complex* VE_code, - const gr_complex* E_code, - const gr_complex* P_code, - const gr_complex* L_code, - const gr_complex* VL_code, - gr_complex* VE_out, - gr_complex* E_out, - gr_complex* P_out, - gr_complex* L_out, - gr_complex* VL_out ); - -#if USING_VOLK_CW_EPL_CORR_CUSTOM - void Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out); -#endif - -private: - unsigned long next_power_2(unsigned long v); -}; -#endif - diff --git a/src/algorithms/tracking/libs/volk_cw_epl_corr.h b/src/algorithms/tracking/libs/volk_cw_epl_corr.h deleted file mode 100644 index 2aed9d2a9..000000000 --- a/src/algorithms/tracking/libs/volk_cw_epl_corr.h +++ /dev/null @@ -1,202 +0,0 @@ -/*! - * \file volk_cw_epl_corr.h - * \brief Implements the carrier wipe-off function and the Early-Prompt-Late - * correlators in a single SSE-enabled loop. - * - * \author Javier Arribas 2012, jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * 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 GNSS_SDR_VOLK_CW_EPL_CORR_H_ -#define GNSS_SDR_VOLK_CW_EPL_CORR_H_ - -#include -#include -#include -#include -#include - -/*! - * TODO: Code the SSE4 version and benchmark it - */ -#ifdef LV_HAVE_SSE3 -#include - - - /*! - \brief Performs the carrier wipe-off mixing and the Early, Prompt, and Late correlation - \param input The input signal input - \param carrier The carrier signal input - \param E_code Early PRN code replica input - \param P_code Early PRN code replica input - \param L_code Early PRN code replica input - \param E_out Early correlation output - \param P_out Early correlation output - \param L_out Early correlation output - \param num_points The number of complex values in vectors - */ -static inline void volk_cw_epl_corr_u(const lv_32fc_t* input, const lv_32fc_t* carrier, const lv_32fc_t* E_code, const lv_32fc_t* P_code, const lv_32fc_t* L_code, lv_32fc_t* E_out, lv_32fc_t* P_out, lv_32fc_t* L_out, unsigned int num_points) -{ - unsigned int number = 0; - const unsigned int halfPoints = num_points / 2; - - lv_32fc_t dotProduct_E; - memset(&dotProduct_E, 0x0, 2*sizeof(float)); - lv_32fc_t dotProduct_P; - memset(&dotProduct_P, 0x0, 2*sizeof(float)); - lv_32fc_t dotProduct_L; - memset(&dotProduct_L, 0x0, 2*sizeof(float)); - - // Aux vars - __m128 x, y, yl, yh, z, tmp1, tmp2, z_E, z_P, z_L; - - z_E = _mm_setzero_ps(); - z_P = _mm_setzero_ps(); - z_L = _mm_setzero_ps(); - - //input and output vectors - //lv_32fc_t* _input_BB = input_BB; - const lv_32fc_t* _input = input; - const lv_32fc_t* _carrier = carrier; - const lv_32fc_t* _E_code = E_code; - const lv_32fc_t* _P_code = P_code; - const lv_32fc_t* _L_code = L_code; - - for(;number < halfPoints; number++) - { - // carrier wipe-off (vector point-to-point product) - x = _mm_loadu_ps((float*)_input); // Load the ar + ai, br + bi as ar,ai,br,bi - y = _mm_loadu_ps((float*)_carrier); // Load the cr + ci, dr + di as cr,ci,dr,di - - yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr - yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di - - tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di - - z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di - - //_mm_storeu_ps((float*)_input_BB,z); // Store the results back into the _input_BB container - - // correlation E,P,L (3x vector scalar product) - // Early - //x = _mm_load_ps((float*)_input_BB); // Load the ar + ai, br + bi as ar,ai,br,bi - x = z; - - y = _mm_load_ps((float*)_E_code); // Load the cr + ci, dr + di as cr,ci,dr,di - - yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr - yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di - - tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di - - z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di - - z_E = _mm_add_ps(z_E, z); // Add the complex multiplication results together - - // Prompt - //x = _mm_load_ps((float*)_input_BB); // Load the ar + ai, br + bi as ar,ai,br,bi - y = _mm_load_ps((float*)_P_code); // Load the cr + ci, dr + di as cr,ci,dr,di - - yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr - yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di - - z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di - - z_P = _mm_add_ps(z_P, z); // Add the complex multiplication results together - - // Late - //x = _mm_load_ps((float*)_input_BB); // Load the ar + ai, br + bi as ar,ai,br,bi - y = _mm_load_ps((float*)_L_code); // Load the cr + ci, dr + di as cr,ci,dr,di - - yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr - yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr - - x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br - - tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di - - z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di - - z_L = _mm_add_ps(z_L, z); // Add the complex multiplication results together - - /*pointer increment*/ - _carrier += 2; - _input += 2; - //_input_BB += 2; - _E_code += 2; - _P_code += 2; - _L_code +=2; - } - - __VOLK_ATTR_ALIGNED(16) lv_32fc_t dotProductVector_E[2]; - __VOLK_ATTR_ALIGNED(16) lv_32fc_t dotProductVector_P[2]; - __VOLK_ATTR_ALIGNED(16) lv_32fc_t dotProductVector_L[2]; - //__VOLK_ATTR_ALIGNED(16) lv_32fc_t _input_BB; - - _mm_store_ps((float*)dotProductVector_E,z_E); // Store the results back into the dot product vector - _mm_store_ps((float*)dotProductVector_P,z_P); // Store the results back into the dot product vector - _mm_store_ps((float*)dotProductVector_L,z_L); // Store the results back into the dot product vector - - dotProduct_E += ( dotProductVector_E[0] + dotProductVector_E[1] ); - dotProduct_P += ( dotProductVector_P[0] + dotProductVector_P[1] ); - dotProduct_L += ( dotProductVector_L[0] + dotProductVector_L[1] ); - - if((num_points % 2) != 0) - { - //_input_BB = (*_input) * (*_carrier); - dotProduct_E += (*_input) * (*_E_code)*(*_carrier); - dotProduct_P += (*_input) * (*_P_code)*(*_carrier); - dotProduct_L += (*_input) * (*_L_code)*(*_carrier); - } - - *E_out = dotProduct_E; - *P_out = dotProduct_P; - *L_out = dotProduct_L; -} - - -#endif /* LV_HAVE_SSE */ -#endif /* GNSS_SDR_VOLK_CW_EPL_CORR_H_ */