diff --git a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc index cf9130767..cba4e5439 100644 --- a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc +++ b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc @@ -108,6 +108,12 @@ Ad9361FpgaSignalSource::Ad9361FpgaSignalSource(ConfigurationInterface* configura scale_dds_dbfs_); } + // turn switch to A/D position + std::string default_device_name = "/dev/uio13"; + std::string device_name = configuration->property(role + ".devicename", default_device_name); + int switch_position = configuration->property(role + ".switch_position", 0); + switch_fpga = std::make_shared (device_name); + switch_fpga->set_switch_position(switch_position); } diff --git a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.h b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.h index 5371f7faf..6110a63c3 100644 --- a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.h +++ b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.h @@ -33,6 +33,7 @@ #define GNSS_SDR_AD9361_FPGA_SIGNAL_SOURCE_H_ #include "gnss_block_interface.h" +#include "fpga_switch.h" #include #include @@ -112,6 +113,8 @@ private: std::string dump_filename_; boost::shared_ptr queue_; + + std::shared_ptr switch_fpga; }; #endif /*GNSS_SDR_AD9361_FPGA_SIGNAL_SOURCE_H_*/ diff --git a/src/algorithms/signal_source/libs/CMakeLists.txt b/src/algorithms/signal_source/libs/CMakeLists.txt index 45131f918..5617be70a 100644 --- a/src/algorithms/signal_source/libs/CMakeLists.txt +++ b/src/algorithms/signal_source/libs/CMakeLists.txt @@ -58,6 +58,10 @@ set (SIGNAL_SOURCE_LIB_SOURCES rtl_tcp_dongle_info.cc ${OPT_SIGNAL_SOURCE_LIB_SOURCES}) +if(ENABLE_FPGA) + SET(SIGNAL_SOURCE_LIB_SOURCES ${SIGNAL_SOURCE_LIB_SOURCES} fpga_switch.cc) +endif(ENABLE_FPGA) + include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ${Boost_INCLUDE_DIRS} diff --git a/src/algorithms/signal_source/libs/fpga_switch.cc b/src/algorithms/signal_source/libs/fpga_switch.cc new file mode 100644 index 000000000..e349a3c53 --- /dev/null +++ b/src/algorithms/signal_source/libs/fpga_switch.cc @@ -0,0 +1,136 @@ +/*! + * \file fpga_switch.cc + * \brief Switch that connects the HW accelerator queues to the analog front end or the DMA. + * \authors
    + *
  • Marc Majoral, 2017. mmajoral(at)cttc.cat + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * Class that controls a switch in the FPGA + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 "fpga_switch.h" + +#include + +// FPGA stuff +#include + +// libraries used by DMA test code and GIPO test code +#include +#include +#include +#include + +// libraries used by DMA test code +#include +#include +#include +#include + +// libraries used by GPIO test code +#include +#include +#include + +// logging +#include + +// string manipulation +#include + +// constants +#define PAGE_SIZE 0x10000 +#define TEST_REGISTER_TRACK_WRITEVAL 0x55AA + +fpga_switch::fpga_switch(std::string device_name) +{ + if ((d_device_descriptor = open(device_name.c_str(), O_RDWR | O_SYNC)) == -1) + { + LOG(WARNING) << "Cannot open deviceio" << device_name; + printf("switch memory successfully mapped\n"); + } + d_map_base = reinterpret_cast(mmap(NULL, PAGE_SIZE, + PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0)); + + if (d_map_base == reinterpret_cast(-1)) + { + LOG(WARNING) << "Cannot map the FPGA switch module into tracking memory"; + printf("could not map switch memory\n"); + } + + // sanity check : check test register + unsigned writeval = TEST_REGISTER_TRACK_WRITEVAL; + unsigned readval; + readval = fpga_switch::fpga_switch_test_register(writeval); + if (writeval != readval) + { + LOG(WARNING) << "Test register sanity check failed"; + } + else + { + LOG(INFO) << "Test register sanity check success !"; + } + + DLOG(INFO) << "Switch FPGA class created"; +} + +fpga_switch::~fpga_switch() +{ + close_device(); +} + +void fpga_switch::set_switch_position(int switch_position) +{ + d_map_base[0] = switch_position; +} + +unsigned fpga_switch::fpga_switch_test_register( + unsigned writeval) +{ + unsigned readval; + // write value to test register + d_map_base[3] = writeval; + // read value from test register + readval = d_map_base[3]; + // return read value + return readval; +} + +void fpga_switch::close_device() +{ + unsigned * aux = const_cast(d_map_base); + if (munmap(static_cast(aux), PAGE_SIZE) == -1) + { + printf("Failed to unmap memory uio\n"); + } + + close(d_device_descriptor); +} + + diff --git a/src/algorithms/signal_source/libs/fpga_switch.h b/src/algorithms/signal_source/libs/fpga_switch.h new file mode 100644 index 000000000..c3ce2fbba --- /dev/null +++ b/src/algorithms/signal_source/libs/fpga_switch.h @@ -0,0 +1,61 @@ +/*! + * \file fpga_switch.h + * \brief Switch that connects the HW accelerator queues to the analog front end or the DMA. + * \authors
    + *
  • Marc Majoral, 2017. mmajoral(at)cttc.cat + *
  • Javier Arribas, 2016. jarribas(at)cttc.es + *
+ * + * Class that controls a switch in the FPGA + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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_FPGA_SWITCH_H_ +#define GNSS_SDR_FPGA_SWITCH_H_ + +#include + +#define MAX_LENGTH_DEVICEIO_NAME 50 + +class fpga_switch +{ +public: + fpga_switch(std::string device_name); + ~fpga_switch(); + void set_switch_position(int switch_position); + +private: + int d_device_descriptor; // driver descriptor + volatile unsigned *d_map_base; // driver memory map + + // private functions + unsigned fpga_switch_test_register(unsigned writeval); + void close_device(void); + +}; + +#endif /* GNSS_SDR_FPGA_SWITCH_H_ */ diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc index a6ebb9bec..f346ff8bc 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc @@ -173,11 +173,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc( d_carrier_phase_step_rad = 0.0; set_relative_rate(1.0 / static_cast(d_vector_length)); - - d_first_time = 1; - - - } void Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::start_tracking() @@ -246,16 +241,14 @@ void Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::start_tracking() d_code_phase_samples = d_acq_code_phase_samples; std::string sys_ = &d_acquisition_gnss_synchro->System; sys = sys_.substr(0,1); - //std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << "Tracking of GPS L1 C/A signal started 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 d_pull_in = true; - multicorrelator_fpga_8sc->lock_channel(); d_enable_tracking = true; //do it in the end to avoid starting running tracking before finishing this function 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; - d_first_time = 1; } Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::~Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc() @@ -275,10 +268,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::~Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc() { volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); - //volk_gnsssdr_free(d_ca_code); delete[] d_Prompt_buffer; - //multicorrelator_cpu.free(); - //volk_gnsssdr_free(d_ca_code_16sc); multicorrelator_fpga_8sc->free(); } catch(const std::exception & ex) @@ -297,8 +287,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib int counter_corr_0_out; int sample_counter; - // samples offset -// int samples_offset; unsigned absolute_samples_offset; // int kk2; // process vars @@ -308,7 +296,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib double code_error_filt_chips = 0.0; int next_prn_length_samples = d_current_prn_length_samples; -// int offset_prn_samples = 0; // Block input data and block output stream pointers Gnss_Synchro **out = reinterpret_cast(&output_items[0]); @@ -324,6 +311,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib if (d_pull_in == true) { d_pull_in = false; + multicorrelator_fpga_8sc->lock_channel(); unsigned counter_value = multicorrelator_fpga_8sc->read_sample_counter(); unsigned num_frames = ceil((counter_value - current_synchro_data.Acq_samplestamp_samples - current_synchro_data.Acq_delay_samples)/d_correlation_length_samples); absolute_samples_offset = current_synchro_data.Acq_delay_samples + current_synchro_data.Acq_samplestamp_samples + num_frames*d_correlation_length_samples; @@ -382,7 +370,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); next_prn_length_samples = round(K_blk_samples); -// offset_prn_samples = next_prn_length_samples - d_current_prn_length_samples; //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] @@ -426,15 +413,12 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - d_debug_loss_of_track = 1; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine multicorrelator_fpga_8sc->unlock_channel(); - - - } + } } // ########### Output the tracking data to navigation and PVT ########## @@ -535,14 +519,13 @@ int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::general_work (int noutput_items __attrib if (d_enable_tracking == true) { - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false - //return 0; // debug + return 1; } else { return 0; } - + } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h index c497c08f2..f7d2141c9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h @@ -181,7 +181,6 @@ private: double d_K_blk_samples_previous; int d_offset_sample_previous; - int d_first_time; int d_kk = 0; int d_numsamples_debug = 990; @@ -194,7 +193,6 @@ private: int d_counter_corr_0_in = 0; int d_counter_corr_0_out = 0; int d_sample_counter_inc = 0; - int d_debug_loss_of_track = 0; }; #endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_SC_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 1cc68dabb..88c4eb2a5 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -47,7 +47,6 @@ set(TRACKING_LIB_SOURCES if(ENABLE_FPGA) SET(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator_8sc.cc) - SET(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator_real_codes_8sc.cc) endif(ENABLE_FPGA) include_directories( diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc b/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc index dd6f208ce..ce2275422 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc @@ -95,23 +95,18 @@ void fpga_multicorrelator_8sc::set_initial_sample(int samples_offset) d_map_base[13] = d_initial_sample_counter; } -//bool fpga_multicorrelator_8sc::set_local_code_and_taps(int code_length_chips, -// const int* local_code_in, float *shifts_chips, int PRN) -bool fpga_multicorrelator_8sc::set_local_code_and_taps(int code_length_chips, +void fpga_multicorrelator_8sc::set_local_code_and_taps(int code_length_chips, float *shifts_chips, int PRN) { - //d_local_code_in = local_code_in; + d_shifts_chips = shifts_chips; d_code_length_chips = code_length_chips; fpga_multicorrelator_8sc::fpga_configure_tracking_gps_local_code(PRN); - return true; } -bool fpga_multicorrelator_8sc::set_output_vectors(gr_complex* corr_out) +void fpga_multicorrelator_8sc::set_output_vectors(gr_complex* corr_out) { - // Save CPU pointers d_corr_out = corr_out; - return true; } void fpga_multicorrelator_8sc::update_local_code(float rem_code_phase_chips) @@ -122,24 +117,20 @@ void fpga_multicorrelator_8sc::update_local_code(float rem_code_phase_chips) } -bool fpga_multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( +void fpga_multicorrelator_8sc::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_code(rem_code_phase_chips); d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad; d_code_phase_step_chips = code_phase_step_chips; d_phase_step_rad = phase_step_rad; d_correlator_length_samples = signal_length_samples; - -// if (first_time == 1) -// { - fpga_multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); - fpga_multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); -// first_time = 0; -// } - + fpga_multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); + fpga_multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); fpga_multicorrelator_8sc::fpga_launch_multicorrelator_fpga(); int irq_count; ssize_t nb; @@ -150,7 +141,6 @@ bool fpga_multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( printf("Tracking_module Interrupt number %d\n", irq_count); } fpga_multicorrelator_8sc::read_tracking_gps_results(); - return true; } fpga_multicorrelator_8sc::fpga_multicorrelator_8sc(int n_correlators, @@ -186,7 +176,6 @@ fpga_multicorrelator_8sc::fpga_multicorrelator_8sc(int n_correlators, d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS*NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) { - //gps_l1_ca_code_gen_int(&d_ca_codes[GPS_L1_CA_CODE_LENGTH_CHIPS*(PRN -1)], PRN, 0); gps_l1_ca_code_gen_int(&d_ca_codes[(int(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); } DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h b/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h index 0b726e121..9bf44536e 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h @@ -51,15 +51,19 @@ public: fpga_multicorrelator_8sc(int n_correlators, std::string device_name, unsigned int device_base); ~fpga_multicorrelator_8sc(); - bool set_output_vectors(gr_complex* corr_out); + //bool set_output_vectors(gr_complex* corr_out); + void set_output_vectors(gr_complex* corr_out); // bool set_local_code_and_taps( // int code_length_chips, const int* local_code_in, // float *shifts_chips, int PRN); - bool set_local_code_and_taps( + //bool set_local_code_and_taps( + void set_local_code_and_taps( int code_length_chips, float *shifts_chips, int PRN); - bool set_output_vectors(lv_16sc_t* corr_out); - void update_local_code(float rem_code_phase_chips);bool Carrier_wipeoff_multicorrelator_resampler( + //bool set_output_vectors(lv_16sc_t* corr_out); + void update_local_code(float rem_code_phase_chips); + //bool Carrier_wipeoff_multicorrelator_resampler( + void 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(); diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.cc b/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.cc deleted file mode 100644 index 942cadaff..000000000 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.cc +++ /dev/null @@ -1,148 +0,0 @@ -/*! - * \file fpga_multicorrelator_real_codes_8sc.cc - * \brief High optimized CPU vector multiTAP correlator class with real-valued local codes - * \authors
    - *
  • Javier Arribas, 2015. jarribas(at)cttc.es - *
  • Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com - *
- * - * Class that implements a high optimized vector multiTAP correlator class for CPUs - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2017 (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_real_codes.h" -#include "fpga_multicorrelator_real_codes_8sc.h" -#include -#include -#include - - -fpga_multicorrelator_real_codes_8sc::fpga_multicorrelator_real_codes_8sc() -{ - d_sig_in = nullptr; - d_local_code_in = nullptr; - d_shifts_chips = nullptr; - d_corr_out = nullptr; - d_local_codes_resampled = nullptr; - d_code_length_chips = 0; - d_n_correlators = 0; -} - - -fpga_multicorrelator_real_codes_8sc::~fpga_multicorrelator_real_codes_8sc() -{ - if(d_local_codes_resampled != nullptr) - { - fpga_multicorrelator_real_codes_8sc::free(); - } -} - - -bool fpga_multicorrelator_real_codes_8sc::init( - int max_signal_length_samples, - int n_correlators) -{ - // ALLOCATE MEMORY FOR INTERNAL vectors - size_t size = max_signal_length_samples * sizeof(float); - - d_local_codes_resampled = static_cast(volk_gnsssdr_malloc(n_correlators * sizeof(float*), volk_gnsssdr_get_alignment())); - for (int n = 0; n < n_correlators; n++) - { - d_local_codes_resampled[n] = static_cast(volk_gnsssdr_malloc(size, volk_gnsssdr_get_alignment())); - } - d_n_correlators = n_correlators; - return true; -} - - - -bool fpga_multicorrelator_real_codes_8sc::set_local_code_and_taps( - int code_length_chips, - const float* 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 fpga_multicorrelator_real_codes_8sc::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 fpga_multicorrelator_real_codes_8sc::update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips) -{ - volk_gnsssdr_32f_xn_resampler_32f_xn(d_local_codes_resampled, - d_local_code_in, - rem_code_phase_chips, - code_phase_step_chips, - d_shifts_chips, - d_code_length_chips, - d_n_correlators, - correlator_length_samples); -} - - -bool fpga_multicorrelator_real_codes_8sc::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_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips); - // Regenerate phase at each call in order to avoid numerical issues - lv_32fc_t phase_offset_as_complex[1]; - phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad)); - // call VOLK_GNSSSDR kernel - volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0, - phase_step_rad)), phase_offset_as_complex, (const float**)d_local_codes_resampled, d_n_correlators, signal_length_samples); - return true; -} - - -bool fpga_multicorrelator_real_codes_8sc::free() -{ - // Free memory - if (d_local_codes_resampled != nullptr) - { - for (int n = 0; n < d_n_correlators; n++) - { - volk_gnsssdr_free(d_local_codes_resampled[n]); - } - volk_gnsssdr_free(d_local_codes_resampled); - d_local_codes_resampled = nullptr; - } - return true; -} - - diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.h b/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.h deleted file mode 100644 index 94d855ff8..000000000 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_real_codes_8sc.h +++ /dev/null @@ -1,73 +0,0 @@ -/*! - * \file fpga_multicorrelator_real_codes_8sc.h - * \brief High optimized CPU vector multiTAP correlator class using real-valued local codes - * \authors
    - *
  • Javier Arribas, 2015. jarribas(at)cttc.es - *
  • Cillian O'Driscoll, 2017, cillian.odriscoll(at)gmail.com - *
- * - * Class that implements a high optimized vector multiTAP correlator class for CPUs - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2017 (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_CPU_MULTICORRELATOR_REAL_CODES_H_ -//#define GNSS_SDR_CPU_MULTICORRELATOR_REAL_CODES_H_ - -#ifndef GNSS_SDR_FPGA_MULTICORRELATOR_REAL_CODES_8SC_H_ -#define GNSS_SDR_FPGA_MULTICORRELATOR_REAL_CODES_8SC_H_ - - -#include - -/*! - * \brief Class that implements carrier wipe-off and correlators. - */ -class fpga_multicorrelator_real_codes_8sc -{ -public: - fpga_multicorrelator_real_codes_8sc(); - ~fpga_multicorrelator_real_codes_8sc(); - bool init(int max_signal_length_samples, int n_correlators); - bool set_local_code_and_taps(int code_length_chips, const float* 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); - 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; - float **d_local_codes_resampled; - const float *d_local_code_in; - std::complex *d_corr_out; - float *d_shifts_chips; - int d_code_length_chips; - int d_n_correlators; -}; - - -#endif /* GNSS_SDR_FPGA_MULTICORRELATOR_REAL_CODES_8SC_H_ */ -