From 05937e69952e5cc9fd23b0945130e867f2b62b06 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 11 Jan 2016 12:26:45 +0100 Subject: [PATCH 1/3] bug fix in interleaved byte to complex data type adapter (incorrect scaled output when char to short int conversion was used, now replaced the two-step process by single GNURadio block interleaver char to complex) --- .../adapters/ibyte_to_complex.cc | 18 ++++++------------ .../adapters/ibyte_to_complex.h | 7 +++---- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.cc b/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.cc index d93055b05..77ac17995 100644 --- a/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.cc +++ b/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.cc @@ -56,11 +56,9 @@ IbyteToComplex::IbyteToComplex(ConfigurationInterface* configuration, std::strin size_t item_size = sizeof(gr_complex); - gr_interleaved_short_to_complex_ = gr::blocks::interleaved_short_to_complex::make(); - gr_char_to_short_ = gr::blocks::char_to_short::make(); + gr_interleaved_char_to_complex_ = gr::blocks::interleaved_char_to_complex::make(); - DLOG(INFO) << "data_type_adapter_(" << gr_interleaved_short_to_complex_->unique_id() << ")"; - DLOG(INFO) << "data_type_adapter_(" << gr_char_to_short_->unique_id() << ")"; + DLOG(INFO) << "data_type_adapter_(" << gr_interleaved_char_to_complex_->unique_id() << ")"; if (dump_) { @@ -77,22 +75,18 @@ IbyteToComplex::~IbyteToComplex() void IbyteToComplex::connect(gr::top_block_sptr top_block) { - top_block->connect(gr_char_to_short_, 0, gr_interleaved_short_to_complex_ , 0); - - if (dump_) { - top_block->connect(gr_interleaved_short_to_complex_, 0, file_sink_, 0); + top_block->connect(gr_interleaved_char_to_complex_, 0, file_sink_, 0); } } void IbyteToComplex::disconnect(gr::top_block_sptr top_block) { - top_block->disconnect(gr_char_to_short_, 0, gr_interleaved_short_to_complex_ , 0); if (dump_) { - top_block->disconnect(gr_interleaved_short_to_complex_, 0, file_sink_, 0); + top_block->disconnect(gr_interleaved_char_to_complex_, 0, file_sink_, 0); } } @@ -100,14 +94,14 @@ void IbyteToComplex::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr IbyteToComplex::get_left_block() { - return gr_char_to_short_; + return gr_interleaved_char_to_complex_; } gr::basic_block_sptr IbyteToComplex::get_right_block() { - return gr_interleaved_short_to_complex_; + return gr_interleaved_char_to_complex_; } diff --git a/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.h b/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.h index 83ea734fb..5abe0441d 100644 --- a/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.h +++ b/src/algorithms/data_type_adapter/adapters/ibyte_to_complex.h @@ -32,10 +32,10 @@ #define GNSS_SDR_IBYTE_TO_COMPLEX_H_ #include -#include -#include +#include #include #include +#include "gnss_synchro.h" #include "gnss_block_interface.h" @@ -74,8 +74,7 @@ public: gr::basic_block_sptr get_right_block(); private: - gr::blocks::interleaved_short_to_complex::sptr gr_interleaved_short_to_complex_; - gr::blocks::char_to_short::sptr gr_char_to_short_; + gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex_; ConfigurationInterface* config_; bool dump_; std::string dump_filename_; From 5ba12c6fdb1f711b79faba628593b8c3566e0f31 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 11 Jan 2016 16:49:37 +0100 Subject: [PATCH 2/3] Added 16 bits short int complex ultra-fast resampler, carrier wipe-off and correlator kernels (250% faster than gr_complex) enabled in tracking GPS_L1_CA_DLL_PLL_C_Aid_Tracking_16sc tracking --- conf/gnss-sdr_Hybrid_byte_sim.conf | 2 +- .../tracking/adapters/CMakeLists.txt | 3 +- .../gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc | 159 +++++ .../gps_l1_ca_dll_pll_c_aid_tracking_16sc.h | 114 ++++ .../tracking/gnuradio_blocks/CMakeLists.txt | 5 +- ...ps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc | 612 ++++++++++++++++++ ...gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h | 189 ++++++ .../gps_l1_ca_dll_pll_tracking_gpu_cc_old.cc | 606 +++++++++++++++++ src/algorithms/tracking/libs/CMakeLists.txt | 1 + .../tracking/libs/cpu_multicorrelator_16sc.cc | 194 ++++++ .../tracking/libs/cpu_multicorrelator_16sc.h | 73 +++ .../tracking/libs/saturated_arithmetic.h | 31 + .../libs/volk_gnsssdr_16ic_resampler_16ic.h | 171 +++++ .../libs/volk_gnsssdr_16ic_x2_dot_prod_16ic.h | 152 +++++ .../libs/volk_gnsssdr_16ic_x2_multiply_16ic.h | 117 ++++ .../volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h | 175 +++++ .../volk_gnsssdr_16ic_xn_resampler_16ic_xn.h | 172 +++++ src/core/receiver/gnss_block_factory.cc | 13 + 18 files changed, 2784 insertions(+), 5 deletions(-) create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.h create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc_old.cc create mode 100644 src/algorithms/tracking/libs/cpu_multicorrelator_16sc.cc create mode 100644 src/algorithms/tracking/libs/cpu_multicorrelator_16sc.h create mode 100644 src/algorithms/tracking/libs/saturated_arithmetic.h create mode 100644 src/algorithms/tracking/libs/volk_gnsssdr_16ic_resampler_16ic.h create mode 100644 src/algorithms/tracking/libs/volk_gnsssdr_16ic_x2_dot_prod_16ic.h create mode 100644 src/algorithms/tracking/libs/volk_gnsssdr_16ic_x2_multiply_16ic.h create mode 100644 src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h create mode 100644 src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_resampler_16ic_xn.h diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index 7ab266275..9855dcae0 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -158,7 +158,7 @@ Resampler.sample_freq_out=4000000 ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=12 +Channels_1C.count=1 ;#count: Number of available Galileo satellite channels. Channels_1B.count=0 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 993c578bd..8b35e506f 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -32,9 +32,10 @@ set(TRACKING_ADAPTER_SOURCES gps_l1_ca_tcp_connector_tracking.cc galileo_e5a_dll_pll_tracking.cc gps_l2_m_dll_pll_tracking.cc + gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc ${OPT_TRACKING_ADAPTERS} ) - + include_directories( $(CMAKE_CURRENT_SOURCE_DIR) ${CMAKE_SOURCE_DIR}/src/core/system_parameters diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc new file mode 100644 index 000000000..c89158b7c --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc @@ -0,0 +1,159 @@ +/*! + * \file gps_l1_ca_dll_pll_c_aid_tracking_16sc.cc + * \brief Implementation of an adapter of a DLL+PLL tracking loop block + * for GPS L1 C/A to a TrackingInterface + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * 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_pll_c_aid_tracking_16sc.h" +#include +#include "GPS_L1_CA.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +GpsL1CaDllPllCAidTracking16sc::GpsL1CaDllPllCAidTracking16sc( + 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 dll_bw_hz; + float early_late_space_chips; + 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); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + 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_pll_c_aid_make_tracking_16sc_cc( + f_if, + fs_in, + vector_length, + queue_, + dump, + dump_filename, + 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() << ")"; +} + + +GpsL1CaDllPllCAidTracking16sc::~GpsL1CaDllPllCAidTracking16sc() +{} + + +void GpsL1CaDllPllCAidTracking16sc::start_tracking() +{ + tracking_->start_tracking(); +} + +/* + * Set tracking channel unique ID + */ +void GpsL1CaDllPllCAidTracking16sc::set_channel(unsigned int channel) +{ + channel_ = channel; + tracking_->set_channel(channel); +} + +/* + * Set tracking channel internal queue + */ +void GpsL1CaDllPllCAidTracking16sc::set_channel_queue( + concurrent_queue *channel_internal_queue) +{ + channel_internal_queue_ = channel_internal_queue; + tracking_->set_channel_queue(channel_internal_queue_); +} + +void GpsL1CaDllPllCAidTracking16sc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + tracking_->set_gnss_synchro(p_gnss_synchro); +} + +void GpsL1CaDllPllCAidTracking16sc::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 GpsL1CaDllPllCAidTracking16sc::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 GpsL1CaDllPllCAidTracking16sc::get_left_block() +{ + return tracking_; +} + +gr::basic_block_sptr GpsL1CaDllPllCAidTracking16sc::get_right_block() +{ + return tracking_; +} + diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.h new file mode 100644 index 000000000..eb95c7c86 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking_16sc.h @@ -0,0 +1,114 @@ +/*! + * \file gps_l1_ca_dll_pll_c_aid_tracking_16sc.h + * \brief Interface of an adapter of a DLL+PLL tracking loop block + * for GPS L1 C/A to a TrackingInterface + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * 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 + * + * ------------------------------------------------------------------------- + * + * 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_PLL_C_AID_TRACKING_16SC_H_ +#define GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_16SC_H_ + +#include +#include +#include "tracking_interface.h" +#include "gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a code DLL + carrier PLL tracking loop + */ +class GpsL1CaDllPllCAidTracking16sc : public TrackingInterface +{ +public: + + GpsL1CaDllPllCAidTracking16sc(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams, + boost::shared_ptr queue); + + virtual ~GpsL1CaDllPllCAidTracking16sc(); + + std::string role() + { + return role_; + } + + //! Returns "gps_l1_ca_dll_pll_c_aid_tracking_16sc" + std::string implementation() + { + return "gps_l1_ca_dll_pll_c_aid_tracking_16sc"; + } + 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(); + + + /*! + * \brief Set tracking channel unique ID + */ + void set_channel(unsigned int channel); + + /*! + * \brief Set acquisition/tracking common Gnss_Synchro object pointer + * to efficiently exchange synchronization data between acquisition and tracking blocks + */ + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + + /*! + * \brief Set tracking channel internal queue + */ + void set_channel_queue(concurrent_queue *channel_internal_queue); + + void start_tracking(); + +private: + gps_l1_ca_dll_pll_c_aid_tracking_16sc_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 // GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_16SC_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 6e9029e53..b71677bfd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -16,7 +16,6 @@ # along with GNSS-SDR. If not, see . # - if(ENABLE_CUDA) set(OPT_TRACKING_BLOCKS ${OPT_TRACKING_BLOCKS} gps_l1_ca_dll_pll_tracking_gpu_cc.cc) set(OPT_TRACKING_INCLUDES ${OPT_TRACKING_INCLUDES} ${CUDA_INCLUDE_DIRS}) @@ -34,6 +33,7 @@ set(TRACKING_GR_BLOCKS_SOURCES galileo_e5a_dll_pll_tracking_cc.cc gps_l2_m_dll_pll_tracking_cc.cc gps_l1_ca_dll_pll_c_aid_tracking_cc.cc + gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc ${OPT_TRACKING_BLOCKS} ) @@ -48,7 +48,6 @@ include_directories( ${GFlags_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GNURADIO_RUNTIME_INCLUDE_DIRS} - ${VOLK_INCLUDE_DIRS} ${VOLK_GNSSSDR_INCLUDE_DIRS} ${OPT_TRACKING_INCLUDES} ) @@ -61,7 +60,7 @@ 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} ${VOLK_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES} ${OPT_TRACKING_LIBRARIES}) +target_link_libraries(tracking_gr_blocks tracking_lib ${GNURADIO_RUNTIME_LIBRARIES} gnss_sp_libs ${Boost_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES} ${OPT_TRACKING_LIBRARIES}) if(NOT VOLK_GNSSSDR_FOUND) add_dependencies(tracking_gr_blocks volk_gnsssdr_module) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc new file mode 100644 index 000000000..ce45da862 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc @@ -0,0 +1,612 @@ +/*! + * \file gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.cc + * \brief Implementation of a code DLL + carrier PLL tracking block + * \author Javier Arribas, 2015. 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "gps_sdr_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "GPS_L1_CA.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_pll_c_aid_tracking_16sc_cc_sptr +gps_l1_ca_dll_pll_c_aid_make_tracking_16sc_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) +{ + return gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc_sptr(new gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc(if_freq, + fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); +} + + + +void gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } +} + + + +gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) : + gr::block("gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // initialize internal vars + d_queue = queue; + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + d_correlation_length_samples = static_cast(d_vector_length); + + // Initialize tracking ========================================== + d_code_loop_filter.set_DLL_BW(dll_bw_hz); + d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2); + + //--- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the C/A code replica sampled 1x/chip + d_ca_code = static_cast(volk_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment())); + d_ca_code_16sc = static_cast(volk_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(lv_16sc_t), volk_get_alignment())); + + d_in_16sc = static_cast(volk_malloc(2 * d_vector_length * sizeof(lv_16sc_t), volk_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, and Late + + d_correlator_outs_16sc = static_cast(volk_malloc(d_n_correlator_taps*sizeof(lv_16sc_t), volk_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_16sc_t(0,0); + } + + d_local_code_shift_chips = static_cast(volk_malloc(d_n_correlator_taps*sizeof(float), volk_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu_16sc.init(2 * d_correlation_length_samples, d_n_correlator_taps); + + //--- Perform initializations ------------------------------ + // define initial code frequency basis of NCO + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carrier_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + //d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; + + d_enable_tracking = false; + d_pull_in = false; + d_last_seg = 0; + + // 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["S"] = std::string("SBAS"); + + set_relative_rate(1.0 / (static_cast(d_vector_length) * 2.0)); + + d_channel_internal_queue = 0; + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_code_phase_samples = 0.0; + + d_pll_to_dll_assist_secs_Ti = 0.0; + d_rem_code_phase_chips = 0.0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + //set_min_output_buffer((long int)300); +} + + +void gps_l1_ca_dll_pll_c_aid_tracking_16sc_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; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + //doppler effect + // Fd=(C/(C+Vr))*F + double 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_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_correlation_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 * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double 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 * static_cast(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; + + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS)); + + multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_16sc_t(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carrier_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_pll_to_dll_assist_secs_Ti = 0.0; + d_code_phase_samples = d_acq_code_phase_samples; + + 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 + 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; +} + + +gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::~gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc() +{ + d_dump_file.close(); + + volk_free(d_local_code_shift_chips); + volk_free(d_ca_code); + + volk_free(d_in_16sc); + volk_free(d_ca_code_16sc); + volk_free(d_correlator_outs_16sc); + + delete[] d_Prompt_buffer; + multicorrelator_cpu_16sc.free(); +} + + + +int gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // Block input data and block output stream pointers + const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + // process vars + double code_error_chips_Ti = 0.0; + double code_error_filt_chips = 0.0; + double code_error_filt_secs_Ti = 0.0; + double CURRENT_INTEGRATION_TIME_S; + double CORRECTED_INTEGRATION_TIME_S; + double dll_code_error_secs_Ti = 0.0; + double carr_phase_error_secs_Ti = 0.0; + double old_d_rem_code_phase_samples; + if (d_enable_tracking == true) + { + // 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_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + d_sample_counter += samples_offset; //count for the processed samples + d_pull_in = false; + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + *out[0] = current_synchro_data; + consume_each(samples_offset); //shift input to perform alignment with local replica + return 1; + } + + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + + volk_gnsssdr_32fc_convert_16ic(d_in_16sc,in,d_correlation_length_samples); + //std::cout << std::fixed << std::setw( 11 ) << std::setprecision( 6 ); + //std::cout<<"in="< 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 + } + } + + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); + // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast(d_fs_in); + // 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 = GPS_TWO_PI * d_acc_carrier_phase_cycles; + 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_pseudorange = false; + *out[0] = current_synchro_data; + + // ########## 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; + DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! + } + } + else + { + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); + DLOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; + } + } + } + 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; + } + } + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_16sc_t(0,0); + } + + current_synchro_data.System = {'G'}; + current_synchro_data.Flag_valid_pseudorange = false; + *out[0] = current_synchro_data; + } + + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs_16sc[1].real(); + prompt_Q = d_correlator_outs_16sc[1].imag(); + tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "Exception writing trk dump file " << e->what(); + } + } + + consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_correlation_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_pll_c_aid_tracking_16sc_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() << std::endl; + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl; + } + } + } +} + +void gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::set_channel_queue(concurrent_queue *channel_internal_queue) +{ + d_channel_internal_queue = channel_internal_queue; +} + +void gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h new file mode 100644 index 000000000..b49c99e4b --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h @@ -0,0 +1,189 @@ +/*! + * \file gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc.h + * \brief Interface of a code DLL + carrier PLL tracking block + * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * Javier Arribas, 2011. jarribas(at)cttc.es + * + * Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, + * Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * 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_PLL_C_AID_TRACKING_16SC_CC_H +#define GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_16SC_CC_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "concurrent_queue.h" +#include "gps_sdr_signal_processing.h" +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_FLL_PLL_filter.h" +#include "cpu_multicorrelator.h" +#include "cpu_multicorrelator_16sc.h" + +class gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc; + +typedef boost::shared_ptr + gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc_sptr; + +gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc_sptr +gps_l1_ca_dll_pll_c_aid_make_tracking_16sc_cc(long if_freq, + long fs_in, unsigned + int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc: public gr::block +{ +public: + ~gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_tracking(); + void set_channel_queue(concurrent_queue *channel_internal_queue); + + 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_pll_c_aid_tracking_16sc_cc_sptr + gps_l1_ca_dll_pll_c_aid_make_tracking_16sc_cc(long if_freq, + long fs_in, unsigned + int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + gps_l1_ca_dll_pll_c_aid_tracking_16sc_cc(long if_freq, + long fs_in, unsigned + int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + // tracking configuration vars + boost::shared_ptr d_queue; + concurrent_queue *d_channel_internal_queue; + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + int d_last_seg; + long d_if_freq; + long d_fs_in; + + double d_early_late_spc_chips; + int d_n_correlator_taps; + + lv_16sc_t* d_in_16sc; + + gr_complex* d_ca_code; + lv_16sc_t* d_ca_code_16sc; + float* d_local_code_shift_chips; + //gr_complex* d_correlator_outs; + lv_16sc_t* d_correlator_outs_16sc; + //cpu_multicorrelator multicorrelator_cpu; + cpu_multicorrelator_16sc multicorrelator_cpu_16sc; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_code_phase_chips; + double d_rem_carrier_phase_rad; + + // PLL and DLL filter library + Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_FLL_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + + // tracking vars + double d_code_freq_chips; + double d_code_phase_step_chips; + double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; + double d_acc_carrier_phase_cycles; + double d_code_phase_samples; + double d_pll_to_dll_assist_secs_Ti; + + //Integration period in samples + int d_correlation_length_samples; + + //processing samples counters + 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; + + // control vars + bool d_enable_tracking; + bool d_pull_in; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_16SC_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc_old.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc_old.cc new file mode 100644 index 000000000..3fd78c48d --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc_old.cc @@ -0,0 +1,606 @@ +/*! + * \file gps_l1_ca_dll_pll_tracking_gpu_cc.cc + * \brief Implementation of a code DLL + carrier PLL tracking block, GPU ACCELERATED + * \author Javier Arribas, 2015. jarribas(at)cttc.es + * + * 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, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * 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_pll_tracking_gpu_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "gps_sdr_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "GPS_L1_CA.h" +#include "control_message_factory.h" +#include //volk_alignement +// includes +#include + + +/*! + * \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_pll_tracking_gpu_cc_sptr +gps_l1_ca_dll_pll_make_tracking_gpu_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) +{ + return gps_l1_ca_dll_pll_tracking_gpu_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(if_freq, + fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); +} + + +void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call +} + + + +Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + boost::shared_ptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) : + gr::block("Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // initialize internal vars + d_queue = queue; + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + + // Initialize tracking ========================================== + d_code_loop_filter.set_DLL_BW(dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + + //--- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Set GPU flags + cudaSetDeviceFlags(cudaDeviceMapHost); + //allocate host memory + //pinned memory mode - use special function to get OS-pinned memory + int N_CORRELATORS = 3; + // Get space for a vector with the C/A code replica sampled 1x/chip + cudaHostAlloc((void**)&d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS* sizeof(gr_complex)), cudaHostAllocMapped || cudaHostAllocWriteCombined); + // Get space for the resampled early / prompt / late local replicas + cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped || cudaHostAllocWriteCombined); + cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined); + // correlator outputs (scalar) + cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocMapped || cudaHostAllocWriteCombined ); + + //map to EPL pointers + d_Early = &d_corr_outs_gpu[0]; + d_Prompt = &d_corr_outs_gpu[1]; + d_Late = &d_corr_outs_gpu[2]; + + //--- Perform initializations ------------------------------ + multicorrelator_gpu = new cuda_multicorrelator(); + //local code resampler on GPU + multicorrelator_gpu->init_cuda_integrated_resampler(2 * d_vector_length, GPS_L1_CA_CODE_LENGTH_CHIPS, 3); + multicorrelator_gpu->set_input_output_vectors(d_corr_outs_gpu, in_gpu); + + // define initial code frequency basis of NCO + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + //d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; + + d_enable_tracking = false; + d_pull_in = false; + d_last_seg = 0; + + d_current_prn_length_samples = static_cast(d_vector_length); + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + 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["S"] = std::string("SBAS"); + + + set_relative_rate(1.0/((double)d_vector_length*2)); + + d_channel_internal_queue = 0; + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_code_phase_samples = 0.0; + d_acc_code_phase_secs = 0.0; + //set_min_output_buffer((long int)300); +} + + +void Gps_L1_Ca_Dll_Pll_Tracking_GPU_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; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + //doppler effect + // Fd=(C/(C+Vr))*F + double 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_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; + T_chip_mod_seconds = 1.0/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + 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 * static_cast(d_fs_in); + double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds; + double 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 * static_cast(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(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); + + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS, d_ca_code, d_local_code_shift_chips, 3); + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0; + d_rem_carr_phase_rad = 0; + d_acc_carrier_phase_rad = 0; + d_acc_code_phase_secs = 0; + + d_code_phase_samples = d_acq_code_phase_samples; + + 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 + 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; +} + + +Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc() +{ + d_dump_file.close(); + cudaFreeHost(in_gpu); + cudaFreeHost(d_corr_outs_gpu); + cudaFreeHost(d_local_code_shift_chips); + cudaFreeHost(d_ca_code); + multicorrelator_gpu->free_cuda(); + delete(multicorrelator_gpu); + delete[] d_Prompt_buffer; +} + + + +int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // process vars + double carr_error_hz=0.0; + double carr_error_filt_hz=0.0; + double code_error_chips=0.0; + double code_error_filt_chips=0.0; + + // Block input data and block output stream pointers + const gr_complex* in = (gr_complex*) input_items[0]; + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + if (d_enable_tracking == true) + { + // 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_seconds = d_sample_counter_seconds + (((double)samples_offset) / static_cast(d_fs_in)); + d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples + d_pull_in = false; + //std::cout<<" samples_offset="<(d_fs_in); + + //code resampler on GPU (new) + double code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + double rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); + + std::cout<<"rem_code_phase_chips="< 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 + } + } + // ########### 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 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)/static_cast(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 = (static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(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 + + //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast(d_fs_in); + // 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 = 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_pseudorange = false; + *out[0] = current_synchro_data; + + // ########## 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; + DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! + } + } + else + { + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); + DLOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; + } + } + } + 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); + + current_synchro_data.System = {'G'}; + current_synchro_data.Flag_valid_pseudorange = false; + *out[0] = current_synchro_data; + } + + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + 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 + //tmp_float=(float)d_sample_counter; + d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + + //PLL commands + tmp_float = carr_error_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + + //DLL commands + tmp_float = code_error_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + + // AUX vars (for debug purposes) + tmp_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(WARNING) << "Exception writing trk dump file " << e.what(); + } + } + + 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 + //LOG(INFO)<<"GPS tracking output end on CH="<d_channel << " SAMPLE STAMP="<(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() << std::endl; + } + catch (std::ifstream::failure e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl; + } + } + } +} + + + +void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_channel_queue(concurrent_queue *channel_internal_queue) +{ + d_channel_internal_queue = channel_internal_queue; +} + + +void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 505c62685..f15b9b914 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -33,6 +33,7 @@ endif(ENABLE_CUDA) set(TRACKING_LIB_SOURCES correlator.cc cpu_multicorrelator.cc + cpu_multicorrelator_16sc.cc lock_detectors.cc tcp_communication.cc tcp_packet_data.cc diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.cc new file mode 100644 index 000000000..572ec32bc --- /dev/null +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.cc @@ -0,0 +1,194 @@ +/*! + * \file cpu_multicorrelator_16sc.cc + * \brief High optimized CPU vector multiTAP correlator class + * \authors
    + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * Class that implements a high optimized vector multiTAP correlator class for CPUs + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ +#include "cpu_multicorrelator_16sc.h" +#include +#include +#include // fixed point sine and cosine + +#define LV_HAVE_GENERIC +#define LV_HAVE_SSE2 +#include "volk_gnsssdr_16ic_x2_dot_prod_16ic.h" +#include "volk_gnsssdr_16ic_x2_multiply_16ic.h" +#include "volk_gnsssdr_16ic_resampler_16ic.h" +#include "volk_gnsssdr_16ic_xn_resampler_16ic_xn.h" +#include "volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h" + +bool cpu_multicorrelator_16sc::init( + int max_signal_length_samples, + int n_correlators + ) +{ + + // ALLOCATE MEMORY FOR INTERNAL vectors + size_t size = max_signal_length_samples * sizeof(lv_16sc_t); + + // NCO signal + d_nco_in = static_cast(volk_malloc(size, volk_get_alignment())); + + // Doppler-free signal + d_sig_doppler_wiped = static_cast(volk_malloc(size, volk_get_alignment())); + + d_local_codes_resampled = new lv_16sc_t*[n_correlators]; + for (int n = 0; n < n_correlators; n++) + { + d_local_codes_resampled[n] = static_cast(volk_malloc(size, volk_get_alignment())); + } + d_n_correlators = n_correlators; + return true; +} + + + +bool cpu_multicorrelator_16sc::set_local_code_and_taps( + int code_length_chips, + const lv_16sc_t* local_code_in, + float *shifts_chips + ) +{ + d_local_code_in = local_code_in; + d_shifts_chips = shifts_chips; + d_code_length_chips = code_length_chips; + return true; +} + + +bool cpu_multicorrelator_16sc::set_input_output_vectors(lv_16sc_t* corr_out, const lv_16sc_t* sig_in) +{ + // Save CPU pointers + d_sig_in = sig_in; + d_corr_out = corr_out; + return true; +} + + + +void cpu_multicorrelator_16sc::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips) +{ + + float *tmp_code_phases_chips; + tmp_code_phases_chips=static_cast(volk_malloc(d_n_correlators*sizeof(float), volk_get_alignment())); + for (int n=0;n(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips); +// //Take into account that in multitap correlators, the shifts can be negative! +// if (local_code_chip_index < 0.0) local_code_chip_index += d_code_length_chips; +// d_local_codes_resampled[current_correlator_tap][n] = d_local_code_in[static_cast(round(local_code_chip_index))]; +// } +// } +} + + +void cpu_multicorrelator_16sc::update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad) +{ + float sin_f, cos_f; + int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad); + int phase_rad_i = gr::fxpt::float_to_fixed(rem_carr_phase_rad); + + for(int i = 0; i < correlator_length_samples; i++) + { + gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f); + d_nco_in[i] = lv_16sc_t((short int)(cos_f*2.0), (short int)(-sin_f*2.0)); + phase_rad_i += phase_step_rad_i; + } +} + +bool cpu_multicorrelator_16sc::Carrier_wipeoff_multicorrelator_resampler( + float rem_carrier_phase_in_rad, + float phase_step_rad, + float rem_code_phase_chips, + float code_phase_step_chips, + int signal_length_samples) +{ + update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad); + + //std::cout<<"d_nco_in 16sc="< +//#include +static inline int16_t sat_adds16b(int16_t x, int16_t y) +{ +// int16_t ux = x; +// int16_t uy = y; +// int16_t res = ux + uy; +// +// /* Calculate overflowed result. (Don't change the sign bit of ux) */ +// ux = (ux >> 15) + SHRT_MAX; +// +// /* Force compiler to use cmovns instruction */ +// if ((int16_t) ((ux ^ uy) | ~(uy ^ res)) >= 0) +// { +// res = ux; +// } +// +// return res; + + int32_t res = (int32_t) x + (int32_t) y; + + if (res < SHRT_MIN) res = SHRT_MIN; + if (res > SHRT_MAX) res = SHRT_MAX; + + return res; +} + +#endif /*SATURATED_ARITHMETIC_H_*/ diff --git a/src/algorithms/tracking/libs/volk_gnsssdr_16ic_resampler_16ic.h b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_resampler_16ic.h new file mode 100644 index 000000000..458022528 --- /dev/null +++ b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_resampler_16ic.h @@ -0,0 +1,171 @@ +/*! + * \file volk_gnsssdr_16ic_resampler_16ic.h + * \brief Volk protokernel: resample a 16 bits complex vector + * \authors
    + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * Volk protokernel that multiplies two 16 bits vectors (8 bits the real part + * and 8 bits the imaginary part) and accumulates them + * + * ------------------------------------------------------------------------- + * + * 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 INCLUDED_volk_gnsssdr_16ic_resampler_16ic_a_H +#define INCLUDED_volk_gnsssdr_16ic_resampler_16ic_a_H + +#include +#include +#include +//#pragma STDC FENV_ACCESS ON + +#ifdef LV_HAVE_GENERIC + +//int round_int( float r ) { +// return (r > 0.0) ? (r + 0.5) : (r - 0.5); +//} +/*! + \brief Multiplies the two input complex vectors, point-by-point, storing the result in the third vector + \param cVector The vector where the result will be stored + \param aVector One of the vectors to be multiplied + \param bVector One of the vectors to be multiplied + \param num_points The number of complex values in aVector and bVector to be multiplied together, accumulated and stored into cVector + */ +static inline void volk_gnsssdr_16ic_resampler_16ic_generic(lv_16sc_t* result, const lv_16sc_t* local_code, float rem_code_phase_chips ,float code_phase_step_chips, unsigned int num_output_samples, unsigned int code_length_chips) +{ + int local_code_chip_index; + //fesetround(FE_TONEAREST); + for (unsigned int n = 0; n < num_output_samples; n++) + { + // resample code for current tap + local_code_chip_index = round(code_phase_step_chips*static_cast(n) + rem_code_phase_chips-0.5f); + if (local_code_chip_index < 0.0) local_code_chip_index += code_length_chips; + if (local_code_chip_index > (code_length_chips-1)) local_code_chip_index -= code_length_chips; + //std::cout<<"g["<(n) + rem_code_phase_chips-0.5f<<","< +static inline void volk_gnsssdr_16ic_resampler_16ic_sse2(lv_16sc_t* result, const lv_16sc_t* local_code, float rem_code_phase_chips ,float code_phase_step_chips, unsigned int num_output_samples, int code_length_chips)//, int* scratch_buffer, float* scratch_buffer_float) +{ + + _MM_SET_ROUNDING_MODE (_MM_ROUND_NEAREST);//_MM_ROUND_NEAREST, _MM_ROUND_DOWN, _MM_ROUND_UP, _MM_ROUND_TOWARD_ZERO + unsigned int number; + const unsigned int quarterPoints = num_output_samples / 4; + + lv_16sc_t* _result = result; + + __attribute__((aligned(16))) int local_code_chip_index[4]; + __m128 _rem_code_phase,_code_phase_step_chips; + __m128i _code_length_chips,_code_length_chips_minus1; + __m128 _code_phase_out,_code_phase_out_with_offset; + rem_code_phase_chips=rem_code_phase_chips-0.5f; + + _rem_code_phase = _mm_load1_ps(&rem_code_phase_chips); //load float to all four float values in m128 register + _code_phase_step_chips = _mm_load1_ps(&code_phase_step_chips); //load float to all four float values in m128 register + __attribute__((aligned(16))) int four_times_code_length_chips_minus1[4]; + four_times_code_length_chips_minus1[0]=code_length_chips-1; + four_times_code_length_chips_minus1[1]=code_length_chips-1; + four_times_code_length_chips_minus1[2]=code_length_chips-1; + four_times_code_length_chips_minus1[3]=code_length_chips-1; + + __attribute__((aligned(16))) int four_times_code_length_chips[4]; + four_times_code_length_chips[0]=code_length_chips; + four_times_code_length_chips[1]=code_length_chips; + four_times_code_length_chips[2]=code_length_chips; + four_times_code_length_chips[3]=code_length_chips; + + _code_length_chips = _mm_loadu_si128((__m128i*)&four_times_code_length_chips); //load float to all four float values in m128 register + _code_length_chips_minus1 = _mm_loadu_si128((__m128i*)&four_times_code_length_chips_minus1); //load float to all four float values in m128 register + + __m128i negative_indexes, overflow_indexes,_code_phase_out_int, _code_phase_out_int_neg,_code_phase_out_int_over; + + __m128i zero=_mm_setzero_si128(); + + + __attribute__((aligned(16))) float init_idx_float[4] = { 0.0f, 1.0f, 2.0f, 3.0f }; + __m128 _4output_index=_mm_load_ps(init_idx_float); + __attribute__((aligned(16))) float init_4constant_float[4] = { 4.0f, 4.0f, 4.0f, 4.0f }; + __m128 _4constant_float=_mm_load_ps(init_4constant_float); + + //__attribute__((aligned(16))) int output_indexes[4]; + + for(number=0;number < quarterPoints; number++){ + _code_phase_out = _mm_mul_ps(_code_phase_step_chips, _4output_index); //compute the code phase point with the phase step + _code_phase_out_with_offset = _mm_add_ps(_code_phase_out,_rem_code_phase); //add the phase offset + _code_phase_out_int=_mm_cvtps_epi32(_code_phase_out_with_offset); //convert to integer + + negative_indexes=_mm_cmplt_epi32 (_code_phase_out_int, zero); //test for negative values + _code_phase_out_int_neg=_mm_add_epi32(_code_phase_out_int,_code_length_chips); //the negative values branch + //_code_phase_out_int_over=_mm_or_si128(_mm_and_si128(_code_phase_out_int_neg,_code_phase_out_int),_mm_andnot_si128(negative_indexes,_code_phase_out_int)); + _code_phase_out_int_neg=_mm_xor_si128(_code_phase_out_int,_mm_and_si128( negative_indexes,_mm_xor_si128( _code_phase_out_int_neg, _code_phase_out_int ))); + + overflow_indexes=_mm_cmpgt_epi32 (_code_phase_out_int_neg, _code_length_chips_minus1); //test for overflow values + _code_phase_out_int_over=_mm_sub_epi32(_code_phase_out_int_neg,_code_length_chips); //the negative values branch + _code_phase_out_int_over=_mm_xor_si128(_code_phase_out_int_neg,_mm_and_si128( overflow_indexes,_mm_xor_si128( _code_phase_out_int_over, _code_phase_out_int_neg ))); + + _mm_storeu_si128((__m128i*)local_code_chip_index,_code_phase_out_int_over); // Store the results back + + //_mm_store_ps((float*)_scratch_buffer_float,_code_phase_out_with_offset); + + //todo: optimize the local code lookup table with intrinsics, if possible + *_result++=local_code[local_code_chip_index[0]]; + *_result++=local_code[local_code_chip_index[1]]; + *_result++=local_code[local_code_chip_index[2]]; + *_result++=local_code[local_code_chip_index[3]]; + + _4output_index = _mm_add_ps(_4output_index,_4constant_float); + //_scratch_buffer_float+=4; + + } + + for(number = quarterPoints * 4;number < num_output_samples; number++){ + local_code_chip_index[0]=static_cast(code_phase_step_chips*static_cast(number) + rem_code_phase_chips+0.5f); + if (local_code_chip_index[0] < 0.0) local_code_chip_index[0] += code_length_chips-1; + if (local_code_chip_index[0] > (code_length_chips-1)) local_code_chip_index[0] -= code_length_chips; + *_result++=local_code[local_code_chip_index[0]]; + //*_scratch_buffer_float++=code_phase_step_chips*static_cast(number)+rem_code_phase_chips; + } + +// for(unsigned int n=0;n + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + * + * + * Volk protokernel that multiplies two 16 bits vectors (8 bits the real part + * and 8 bits the imaginary part) and accumulates them + * + * ------------------------------------------------------------------------- + * + * 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 INCLUDED_volk_gnsssdr_16ic_x2_dot_prod_16ic_u_H +#define INCLUDED_volk_gnsssdr_16ic_x2_dot_prod_16ic_u_H + +#include +#include +#include +#include +#include "saturated_arithmetic.h" + +#ifdef LV_HAVE_GENERIC +/*! + \brief Multiplies the two input complex vectors and accumulates them, storing the result in the third vector + \param cVector The vector where the accumulated result will be stored + \param aVector One of the vectors to be multiplied and accumulated + \param bVector One of the vectors to be multiplied and accumulated + \param num_points The number of complex values in aVector and bVector to be multiplied together, accumulated and stored into cVector + */ +static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_generic(lv_16sc_t* result, const lv_16sc_t* in_a, const lv_16sc_t* in_b, unsigned int num_points) +{ + result[0]=lv_16sc_t(0,0); + for (unsigned int n=0;n +static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_a_sse2(lv_16sc_t* out, const lv_16sc_t* in_a, const lv_16sc_t* in_b, unsigned int num_points) +{ + + lv_16sc_t dotProduct=lv_16sc_t(0,0); + + const unsigned int sse_iters = num_points / 4; + + const lv_16sc_t* _in_a = in_a; + const lv_16sc_t* _in_b = in_b; + lv_16sc_t* _out = out; + + if (sse_iters>0) + { + + + __m128i a,b,c, c_sr, mask_imag, mask_real, real, imag, imag1,imag2, b_sl, a_sl, realcacc, imagcacc, result; + + realcacc = _mm_setzero_si128(); + imagcacc = _mm_setzero_si128(); + + mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); + mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + + for(unsigned int number = 0;number < sse_iters; number++) + { + //std::complex memory structure: real part -> reinterpret_cast(a)[2*i] + //imaginery part -> reinterpret_cast(a)[2*i + 1] + // a[127:0]=[a3.i,a3.r,a2.i,a2.r,a1.i,a1.r,a0.i,a0.r] + a = _mm_loadu_si128((__m128i*)_in_a); //load (2 byte imag, 2 byte real) x 4 into 128 bits reg + b = _mm_loadu_si128((__m128i*)_in_b); + c=_mm_mullo_epi16 (a, b); // a3.i*b3.i, a3.r*b3.r, .... + + c_sr = _mm_srli_si128 (c, 2); // Shift a right by imm8 bytes while shifting in zeros, and store the results in dst. + real = _mm_subs_epi16 (c,c_sr); + + b_sl = _mm_slli_si128(b,2); // b3.r, b2.i .... + a_sl = _mm_slli_si128(a,2); // a3.r, a2.i .... + + imag1 = _mm_mullo_epi16(a,b_sl); // a3.i*b3.r, .... + imag2 = _mm_mullo_epi16(b,a_sl); // b3.i*a3.r, .... + + imag = _mm_adds_epi16(imag1,imag2); //with saturation aritmetic! + + realcacc = _mm_adds_epi16 (realcacc, real); + imagcacc = _mm_adds_epi16 (imagcacc, imag); + + _in_a += 4; + _in_b += 4; + + } + + + realcacc = _mm_and_si128 (realcacc, mask_real); + imagcacc = _mm_and_si128 (imagcacc, mask_imag); + + result = _mm_or_si128 (realcacc, imagcacc); + + __VOLK_ATTR_ALIGNED(16) lv_16sc_t dotProductVector[4]; + + _mm_storeu_si128((__m128i*)dotProductVector,result); // Store the results back into the dot product vector + + for (int i = 0; i<4; ++i) + { + dotProduct.real(sat_adds16b(dotProduct.real(),dotProductVector[i].real())); + dotProduct.imag(sat_adds16b(dotProduct.imag(),dotProductVector[i].imag())); + } + } + + for (unsigned int i = 0; i<(num_points % 4); ++i) + { + //dotProduct += (*_in_a++) * (*_in_b++); + lv_16sc_t tmp=(*_in_a++) * (*_in_b++); + dotProduct.real(sat_adds16b(dotProduct.real(),tmp.real())); + dotProduct.imag(sat_adds16b(dotProduct.imag(),tmp.imag())); + + } + + *_out = dotProduct; +} +#endif /* LV_HAVE_SSE2 */ + +#endif /*INCLUDED_volk_gnsssdr_16ic_x2_dot_prod_16ic_u_H*/ diff --git a/src/algorithms/tracking/libs/volk_gnsssdr_16ic_x2_multiply_16ic.h b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_x2_multiply_16ic.h new file mode 100644 index 000000000..87102071a --- /dev/null +++ b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_x2_multiply_16ic.h @@ -0,0 +1,117 @@ +/*! + * \file volk_gnsssdr_16ic_x2_dot_prod_16ic.h + * \brief Volk protokernel: multiplies two 16 bits vectors and accumulates them + * \authors
      + *
    • Javier Arribas, 2015. jarribas(at)cttc.es + *
    + * + * Volk protokernel that multiplies two 16 bits vectors (8 bits the real part + * and 8 bits the imaginary part) and accumulates them + * + * ------------------------------------------------------------------------- + * + * 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 INCLUDED_volk_gnsssdr_16ic_x2_multiply_16ic_a_H +#define INCLUDED_volk_gnsssdr_16ic_x2_multiply_16ic_a_H + +#include +#include +#include +#include + +#ifdef LV_HAVE_GENERIC +/*! + \brief Multiplies the two input complex vectors, point-by-point, storing the result in the third vector + \param cVector The vector where the result will be stored + \param aVector One of the vectors to be multiplied + \param bVector One of the vectors to be multiplied + \param num_points The number of complex values in aVector and bVector to be multiplied together, accumulated and stored into cVector + */ +static inline void volk_gnsssdr_16ic_x2_multiply_16ic_generic(lv_16sc_t* result, const lv_16sc_t* in_a, const lv_16sc_t* in_b, unsigned int num_points) +{ + for (unsigned int n=0;n +static inline void volk_gnsssdr_16ic_x2_multiply_16ic_a_sse2(lv_16sc_t* out, const lv_16sc_t* in_a, const lv_16sc_t* in_b, unsigned int num_points) +{ + const unsigned int sse_iters = num_points / 4; + __m128i a,b,c, c_sr, mask_imag, mask_real, real, imag, imag1,imag2, b_sl, a_sl, result; + + mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); + mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + + const lv_16sc_t* _in_a = in_a; + const lv_16sc_t* _in_b = in_b; + lv_16sc_t* _out = out; + for(unsigned int number = 0;number < sse_iters; number++) + { + + //std::complex memory structure: real part -> reinterpret_cast(a)[2*i] + //imaginery part -> reinterpret_cast(a)[2*i + 1] + // a[127:0]=[a3.i,a3.r,a2.i,a2.r,a1.i,a1.r,a0.i,a0.r] + a = _mm_loadu_si128((__m128i*)_in_a); //load (2 byte imag, 2 byte real) x 4 into 128 bits reg + b = _mm_loadu_si128((__m128i*)_in_b); + c=_mm_mullo_epi16 (a, b); // a3.i*b3.i, a3.r*b3.r, .... + + c_sr = _mm_srli_si128 (c, 2); // Shift a right by imm8 bytes while shifting in zeros, and store the results in dst. + real = _mm_subs_epi16 (c,c_sr); + real = _mm_and_si128 (real, mask_real); // a3.r*b3.r-a3.i*b3.i , 0, a3.r*b3.r- a3.i*b3.i + + b_sl = _mm_slli_si128(b,2); // b3.r, b2.i .... + a_sl = _mm_slli_si128(a,2); // a3.r, a2.i .... + + imag1 = _mm_mullo_epi16(a,b_sl); // a3.i*b3.r, .... + imag2 = _mm_mullo_epi16(b,a_sl); // b3.i*a3.r, .... + + imag = _mm_adds_epi16(imag1,imag2); + imag = _mm_and_si128 (imag, mask_imag); // a3.i*b3.r+b3.i*a3.r, 0, ... + + result = _mm_or_si128 (real, imag); + + _mm_storeu_si128((__m128i*)_out, result); + + _in_a += 4; + _in_b += 4; + _out += 4; + + } + + for (unsigned int i = 0; i<(num_points % 4); ++i) + { + *_out++ = (*_in_a++) * (*_in_b++); + } +} +#endif /* LV_HAVE_SSE2 */ + +#endif /*INCLUDED_volk_gnsssdr_16ic_x2_multiply_16ic_a_H*/ diff --git a/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h new file mode 100644 index 000000000..99c456686 --- /dev/null +++ b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h @@ -0,0 +1,175 @@ +/*! + * \file volk_gnsssdr_16ic_x2_dot_prod_16ic.h + * \brief Volk protokernel: multiplies two 16 bits vectors and accumulates them + * \authors
      + *
    • Javier Arribas, 2015. jarribas(at)cttc.es + *
    + * + * Volk protokernel that multiplies two 16 bits vectors (8 bits the real part + * and 8 bits the imaginary part) and accumulates them + * + * ------------------------------------------------------------------------- + * + * 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 INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H +#define INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H + +#include +#include +#include +#include +#include "saturated_arithmetic.h" + +#ifdef LV_HAVE_GENERIC +/*! + \brief Multiplies the two input complex vectors and accumulates them, storing the result in the third vector + \param cVector The vector where the accumulated result will be stored + \param aVector One of the vectors to be multiplied and accumulated + \param bVector One of the vectors to be multiplied and accumulated + \param num_points The number of complex values in aVector and bVector to be multiplied together, accumulated and stored into cVector + */ +static inline void volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_generic(lv_16sc_t* result, const lv_16sc_t* in_common, const lv_16sc_t** in_a, unsigned int num_points, int num_a_vectors) +{ + + + for (int n_vec=0;n_vec +static inline void volk_gnsssdr_16ic_xn_resampler_16ic_xn_sse2(lv_16sc_t** result, const lv_16sc_t* local_code, float* rem_code_phase_chips ,float code_phase_step_chips, unsigned int num_output_samples, unsigned int code_length_chips, int num_out_vectors) +{ + + _MM_SET_ROUNDING_MODE (_MM_ROUND_NEAREST);//_MM_ROUND_NEAREST, _MM_ROUND_DOWN, _MM_ROUND_UP, _MM_ROUND_TOWARD_ZERO + unsigned int number; + const unsigned int quarterPoints = num_output_samples / 4; + + lv_16sc_t** _result = result; + __attribute__((aligned(16))) int local_code_chip_index[4]; + float tmp_rem_code_phase_chips; + __m128 _rem_code_phase,_code_phase_step_chips; + __m128i _code_length_chips,_code_length_chips_minus1; + __m128 _code_phase_out,_code_phase_out_with_offset; + + _code_phase_step_chips = _mm_load1_ps(&code_phase_step_chips); //load float to all four float values in m128 register + __attribute__((aligned(16))) int four_times_code_length_chips_minus1[4]; + four_times_code_length_chips_minus1[0]=code_length_chips-1; + four_times_code_length_chips_minus1[1]=code_length_chips-1; + four_times_code_length_chips_minus1[2]=code_length_chips-1; + four_times_code_length_chips_minus1[3]=code_length_chips-1; + + __attribute__((aligned(16))) int four_times_code_length_chips[4]; + four_times_code_length_chips[0]=code_length_chips; + four_times_code_length_chips[1]=code_length_chips; + four_times_code_length_chips[2]=code_length_chips; + four_times_code_length_chips[3]=code_length_chips; + + _code_length_chips = _mm_loadu_si128((__m128i*)&four_times_code_length_chips); //load float to all four float values in m128 register + _code_length_chips_minus1 = _mm_loadu_si128((__m128i*)&four_times_code_length_chips_minus1); //load float to all four float values in m128 register + + __m128i negative_indexes, overflow_indexes,_code_phase_out_int, _code_phase_out_int_neg,_code_phase_out_int_over; + + __m128i zero=_mm_setzero_si128(); + + __attribute__((aligned(16))) float init_idx_float[4] = { 0.0f, 1.0f, 2.0f, 3.0f }; + __m128 _4output_index=_mm_load_ps(init_idx_float); + __attribute__((aligned(16))) float init_4constant_float[4] = { 4.0f, 4.0f, 4.0f, 4.0f }; + __m128 _4constant_float=_mm_load_ps(init_4constant_float); + + int current_vector=0; + int sample_idx=0; + for(number=0;number < quarterPoints; number++){ + //common to all outputs + _code_phase_out = _mm_mul_ps(_code_phase_step_chips, _4output_index); //compute the code phase point with the phase step + + //output vector dependant (different code phase offset) + for(current_vector=0;current_vector(code_phase_step_chips*static_cast(number) + rem_code_phase_chips[current_vector]); + if (local_code_chip_index[0] < 0.0) local_code_chip_index[0] += code_length_chips-1; + if (local_code_chip_index[0] > (code_length_chips-1)) local_code_chip_index[0] -= code_length_chips; + _result[current_vector][number]=local_code[local_code_chip_index[0]]; + } + + } + +} +#endif /* LV_HAVE_SSE2 */ + +#endif /*INCLUDED_volk_gnsssdr_16ic_xn_resampler_16ic_xn_a_H*/ diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index c5c74f35b..5c17fde12 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -79,6 +79,7 @@ #include "galileo_e5a_noncoherent_iq_acquisition_caf.h" #include "gps_l1_ca_dll_pll_tracking.h" #include "gps_l1_ca_dll_pll_c_aid_tracking.h" +#include "gps_l1_ca_dll_pll_c_aid_tracking_16sc.h" #include "gps_l1_ca_dll_pll_optim_tracking.h" #include "gps_l1_ca_dll_fll_pll_tracking.h" #include "gps_l1_ca_tcp_connector_tracking.h" @@ -1318,6 +1319,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams, queue)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking_16sc") == 0) + { + std::unique_ptr block_(new GpsL1CaDllPllCAidTracking16sc(configuration.get(), role, in_streams, + out_streams, queue)); + block = std::move(block_); + } else if (implementation.compare("GPS_L1_CA_DLL_PLL_Optim_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllOptimTracking(configuration.get(), role, in_streams, @@ -1589,6 +1596,12 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams, queue)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking_16sc") == 0) + { + std::unique_ptr block_(new GpsL1CaDllPllCAidTracking16sc(configuration.get(), role, in_streams, + out_streams, queue)); + block = std::move(block_); + } else if (implementation.compare("GPS_L1_CA_DLL_PLL_Optim_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllOptimTracking(configuration.get(), role, in_streams, From c2676ec49192e02d9009dded343d55719b666b57 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 12 Jan 2016 18:58:00 +0100 Subject: [PATCH 3/3] Added missing includes (to be removed in future versions). Steps towards the integration of new standalone kernels into volk_gnsssdr library. --- conf/gnss-sdr_Hybrid_byte_sim.conf | 4 +-- .../tracking/adapters/CMakeLists.txt | 1 + src/algorithms/tracking/libs/CMakeLists.txt | 3 +- .../tracking/libs/cpu_multicorrelator_16sc.h | 2 +- .../volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h | 28 ++++++++----------- src/core/receiver/CMakeLists.txt | 1 + 6 files changed, 19 insertions(+), 20 deletions(-) diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index 9855dcae0..4e4d558ad 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -158,7 +158,7 @@ Resampler.sample_freq_out=4000000 ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=1 +Channels_1C.count=12 ;#count: Number of available Galileo satellite channels. Channels_1B.count=0 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver @@ -233,7 +233,7 @@ Acquisition_1B.doppler_step=125 ;######### TRACKING GPS CONFIG ############ ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking_16sc ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. Tracking_1C.item_type=gr_complex diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 8b35e506f..2d4855c8c 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -47,6 +47,7 @@ include_directories( ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${GNURADIO_RUNTIME_INCLUDE_DIRS} + ${VOLK_GNSSSDR_INCLUDE_DIRS} ${OPT_TRACKING_INCLUDE_DIRS} ) diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index f15b9b914..e0c7d4d69 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -53,6 +53,7 @@ include_directories( ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${OPT_TRACKING_INCLUDES} + ${VOLK_GNSSSDR_INCLUDE_DIRS} ) if(ENABLE_GENERIC_ARCH) @@ -67,5 +68,5 @@ endif(SSE3_AVAILABLE) file(GLOB TRACKING_LIB_HEADERS "*.h") add_library(tracking_lib ${TRACKING_LIB_SOURCES} ${TRACKING_LIB_HEADERS}) source_group(Headers FILES ${TRACKING_LIB_HEADERS}) -target_link_libraries(tracking_lib ${OPT_TRACKING_LIBRARIES} ${VOLK_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}) +target_link_libraries(tracking_lib ${OPT_TRACKING_LIBRARIES} ${VOLK_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}) add_dependencies(tracking_lib glog-${glog_RELEASE}) diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.h b/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.h index 2863d1f3a..d3fa69042 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.h +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_16sc.h @@ -36,7 +36,7 @@ #define GNSS_SDR_CPU_MULTICORRELATOR_16SC_H_ #include //include original volk first! -#include "volk_gnsssdr/volk_gnsssdr.h" +#include /*! diff --git a/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h index 99c456686..f55a1b0b8 100644 --- a/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h +++ b/src/algorithms/tracking/libs/volk_gnsssdr_16ic_xn_dot_prod_16ic_xn.h @@ -36,10 +36,8 @@ #ifndef INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H #define INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H -#include + #include -#include -#include #include "saturated_arithmetic.h" #ifdef LV_HAVE_GENERIC @@ -52,21 +50,16 @@ */ static inline void volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_generic(lv_16sc_t* result, const lv_16sc_t* in_common, const lv_16sc_t** in_a, unsigned int num_points, int num_a_vectors) { - - for (int n_vec=0;n_vec