Adding GPS L5i acquisition and tracking modules

src/algorithms/acquisition/adapters/CMakeLists.txt

@@ -24,6 +24,7 @@ set(ACQ_ADAPTER_SOURCES
     gps_l1_ca_pcps_tong_acquisition.cc
     gps_l1_ca_pcps_quicksync_acquisition.cc
     gps_l2_m_pcps_acquisition.cc
+    gps_l5i_pcps_acquisition.cc
     galileo_e1_pcps_ambiguous_acquisition.cc
     galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
     galileo_e1_pcps_quicksync_ambiguous_acquisition.cc

src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc

@@ -0,0 +1,387 @@
+/*!
+ * \file gps_l5i pcps_acquisition.cc
+ * \brief Adapts a PCPS acquisition block to an Acquisition Interface for
+ *  GPS L5i signals
+ * \authors <ul>
+ *          <li> Javier Arribas, 2017. jarribas(at)cttc.es
+ *          </ul>
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2017  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "gps_l5i_pcps_acquisition.h"
+#include <boost/math/distributions/exponential.hpp>
+#include <glog/logging.h>
+#include "gps_l5_signal.h"
+#include "GPS_L5.h"
+#include "configuration_interface.h"
+
+
+using google::LogMessage;
+
+GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
+        ConfigurationInterface* configuration, std::string role,
+        unsigned int in_streams, unsigned int out_streams) :
+    role_(role), in_streams_(in_streams), out_streams_(out_streams)
+{
+    configuration_ = configuration;
+    std::string default_item_type = "gr_complex";
+    std::string default_dump_filename = "./data/acquisition.dat";
+
+    LOG(INFO) << "role " << role;
+
+    item_type_ = configuration_->property(role + ".item_type", default_item_type);
+
+    long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
+    fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
+    if_ = configuration_->property(role + ".if", 0);
+    dump_ = configuration_->property(role + ".dump", false);
+    blocking_ = configuration_->property(role + ".blocking", true);
+    doppler_max_ = configuration->property(role + ".doppler_max", 5000);
+
+    bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
+    use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
+
+    max_dwells_ = configuration_->property(role + ".max_dwells", 1);
+
+    dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
+
+    //--- Find number of samples per spreading code -------------------------
+    code_length_ = round(static_cast<double>(fs_in_)
+            / (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
+
+    vector_length_ = code_length_;
+
+    if( bit_transition_flag_ )
+        {
+            vector_length_ *= 2;
+        }
+
+    code_ = new gr_complex[vector_length_];
+
+    if (item_type_.compare("cshort") == 0 )
+        {
+            item_size_ = sizeof(lv_16sc_t);
+            acquisition_sc_ = pcps_make_acquisition_sc(1, max_dwells_,
+                    doppler_max_, if_, fs_in_, code_length_, code_length_,
+                    bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_);
+            DLOG(INFO) << "acquisition(" << acquisition_sc_->unique_id() << ")";
+
+        }
+    else
+        {
+            item_size_ = sizeof(gr_complex);
+            acquisition_cc_ = pcps_make_acquisition_cc(1, max_dwells_,
+                    doppler_max_, if_, fs_in_, code_length_, code_length_,
+                    bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_,
+                    dump_filename_);
+            DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")";
+        }
+
+    stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
+    DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")";
+    
+    if (item_type_.compare("cbyte") == 0)
+        {
+            cbyte_to_float_x2_ = make_complex_byte_to_float_x2();
+            float_to_complex_ = gr::blocks::float_to_complex::make();
+        }
+
+    channel_ = 0;
+    threshold_ = 0.0;
+    doppler_step_ = 0;
+    gnss_synchro_ = 0;
+}
+
+
+GpsL5iPcpsAcquisition::~GpsL5iPcpsAcquisition()
+{
+    delete[] code_;
+}
+
+
+void GpsL5iPcpsAcquisition::set_channel(unsigned int channel)
+{
+    channel_ = channel;
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_channel(channel_);
+        }
+    else
+        {
+            acquisition_cc_->set_channel(channel_);
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::set_threshold(float threshold)
+{
+    float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
+
+    if(pfa == 0.0)
+        {
+            pfa = configuration_->property(role_ + ".pfa", 0.0);
+        }
+    if(pfa == 0.0)
+        {
+            threshold_ = threshold;
+        }
+    else
+        {
+            threshold_ = calculate_threshold(pfa);
+        }
+
+    DLOG(INFO) << "Channel " << channel_ <<" Threshold = " << threshold_;
+
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_threshold(threshold_);
+        }
+    else
+        {
+            acquisition_cc_->set_threshold(threshold_);
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::set_doppler_max(unsigned int doppler_max)
+{
+    doppler_max_ = doppler_max;
+
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_doppler_max(doppler_max_);
+        }
+    else
+        {
+            acquisition_cc_->set_doppler_max(doppler_max_);
+        }
+}
+
+
+// Be aware that Doppler step should be set to 2/(3T) Hz, where T is the coherent integration time (GPS L2 period is 0.02s)
+// Doppler bin minimum size= 33 Hz
+void GpsL5iPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
+{
+    doppler_step_ = doppler_step;
+
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_doppler_step(doppler_step_);
+        }
+    else
+        {
+            acquisition_cc_->set_doppler_step(doppler_step_);
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
+{
+    gnss_synchro_ = gnss_synchro;
+
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_gnss_synchro(gnss_synchro_);
+        }
+    else
+        {
+            acquisition_cc_->set_gnss_synchro(gnss_synchro_);
+        }
+}
+
+
+signed int GpsL5iPcpsAcquisition::mag()
+{
+    if (item_type_.compare("cshort") == 0)
+        {
+            return acquisition_sc_->mag();
+        }
+    else
+        {
+            return acquisition_cc_->mag();
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::init()
+{
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->init();
+        }
+    else
+        {
+            acquisition_cc_->init();
+        }
+
+}
+
+void GpsL5iPcpsAcquisition::set_local_code()
+{
+
+    gps_l5i_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
+    
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_local_code(code_);
+        }
+    else
+        {
+            acquisition_cc_->set_local_code(code_);
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::reset()
+{
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_active(true);
+        }
+    else
+        {
+            acquisition_cc_->set_active(true);
+        }
+}
+
+void GpsL5iPcpsAcquisition::set_state(int state)
+{
+    if (item_type_.compare("cshort") == 0)
+        {
+            acquisition_sc_->set_state(state);
+        }
+    else
+        {
+            acquisition_cc_->set_state(state);
+        }
+}
+
+
+
+float GpsL5iPcpsAcquisition::calculate_threshold(float pfa)
+{
+    //Calculate the threshold
+    unsigned int frequency_bins = 0;
+    for (int doppler = static_cast<int>(-doppler_max_); doppler <= static_cast<int>(doppler_max_); doppler += doppler_step_)
+        {
+            frequency_bins++;
+        }
+    DLOG(INFO) << "Channel " << channel_<< "  Pfa = " << pfa;
+    unsigned int ncells = vector_length_ * frequency_bins;
+    double exponent = 1.0 / static_cast<double>(ncells);
+    double val = pow(1.0 - pfa, exponent);
+    double lambda = double(vector_length_);
+    boost::math::exponential_distribution<double> mydist (lambda);
+    float threshold = static_cast<float>(quantile(mydist,val));
+
+    return threshold;
+}
+
+
+void GpsL5iPcpsAcquisition::connect(gr::top_block_sptr top_block)
+{
+    if (item_type_.compare("gr_complex") == 0)
+        {
+            top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
+        }
+    else if (item_type_.compare("cshort") == 0)
+        {
+            top_block->connect(stream_to_vector_, 0, acquisition_sc_, 0);
+        }
+    else if (item_type_.compare("cbyte") == 0)
+        {
+            top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0);
+            top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1);
+            top_block->connect(float_to_complex_, 0, stream_to_vector_, 0);
+            top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
+        }
+    else
+        {
+            LOG(WARNING) << item_type_ << " unknown acquisition item type";
+        }
+}
+
+
+void GpsL5iPcpsAcquisition::disconnect(gr::top_block_sptr top_block)
+{
+    if (item_type_.compare("gr_complex") == 0)
+        {
+            top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
+        }
+    else if (item_type_.compare("cshort") == 0)
+        {
+            top_block->disconnect(stream_to_vector_, 0, acquisition_sc_, 0);
+        }
+    else if (item_type_.compare("cbyte") == 0)
+        {
+            // Since a byte-based acq implementation is not available,
+            // we just convert cshorts to gr_complex
+            top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0);
+            top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1);
+            top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0);
+            top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
+        }
+    else
+        {
+            LOG(WARNING) << item_type_ << " unknown acquisition item type";
+        }
+}
+
+
+gr::basic_block_sptr GpsL5iPcpsAcquisition::get_left_block()
+{
+    if (item_type_.compare("gr_complex") == 0)
+        {
+            return stream_to_vector_;
+        }
+    else if (item_type_.compare("cshort") == 0)
+        {
+            return stream_to_vector_;
+        }
+    else if (item_type_.compare("cbyte") == 0)
+        {
+            return cbyte_to_float_x2_;
+        }
+    else
+        {
+            LOG(WARNING) << item_type_ << " unknown acquisition item type";
+            return nullptr;
+        }
+}
+
+
+gr::basic_block_sptr GpsL5iPcpsAcquisition::get_right_block()
+{
+    if (item_type_.compare("cshort") == 0)
+        {
+            return acquisition_sc_;
+        }
+    else
+        {
+            return acquisition_cc_;
+        }
+}
+

src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h

@@ -0,0 +1,171 @@
+/*!
+ * \file GPS_L5i_PCPS_Acquisition.h
+ * \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
+ *  GPS L5i signals
+ * \authors <ul>
+ *          <li> Javier Arribas, 2017. jarribas(at)cttc.es
+ *          </ul>
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2017  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_
+#define GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_
+
+#include <string>
+#include <gnuradio/blocks/stream_to_vector.h>
+#include <gnuradio/blocks/float_to_complex.h>
+#include "gnss_synchro.h"
+#include "acquisition_interface.h"
+#include "pcps_acquisition_cc.h"
+#include "pcps_acquisition_sc.h"
+#include "complex_byte_to_float_x2.h"
+#include <volk_gnsssdr/volk_gnsssdr.h>
+
+
+
+class ConfigurationInterface;
+
+/*!
+ * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
+ *  for GPS L5i signals
+ */
+class GpsL5iPcpsAcquisition: public AcquisitionInterface
+{
+public:
+    GpsL5iPcpsAcquisition(ConfigurationInterface* configuration,
+            std::string role, unsigned int in_streams,
+            unsigned int out_streams);
+
+    virtual ~GpsL5iPcpsAcquisition();
+
+    inline std::string role() override
+    {
+        return role_;
+    }
+
+    /*!
+     * \brief Returns "GPS_L5i_PCPS_Acquisition"
+     */
+    inline std::string implementation() override
+    {
+        return "GPS_L5i_PCPS_Acquisition";
+    }
+
+    inline size_t item_size() override
+    {
+        return item_size_;
+    }
+
+    void connect(gr::top_block_sptr top_block) override;
+    void disconnect(gr::top_block_sptr top_block) override;
+    gr::basic_block_sptr get_left_block() override;
+    gr::basic_block_sptr get_right_block() override;
+
+    /*!
+     * \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) override;
+
+    /*!
+     * \brief Set acquisition channel unique ID
+     */
+    void set_channel(unsigned int channel) override;
+
+    /*!
+     * \brief Set statistics threshold of PCPS algorithm
+     */
+    void set_threshold(float threshold) override;
+
+    /*!
+     * \brief Set maximum Doppler off grid search
+     */
+    void set_doppler_max(unsigned int doppler_max) override;
+
+    /*!
+     * \brief Set Doppler steps for the grid search
+     */
+    void set_doppler_step(unsigned int doppler_step) override;
+
+    /*!
+     * \brief Initializes acquisition algorithm.
+     */
+    void init() override;
+
+    /*!
+     * \brief Sets local code for GPS L2/M PCPS acquisition algorithm.
+     */
+    void set_local_code() override;
+
+    /*!
+     * \brief Returns the maximum peak of grid search
+     */
+    signed int mag() override;
+
+    /*!
+     * \brief Restart acquisition algorithm
+     */
+    void reset() override;
+
+    /*!
+     * \brief If state = 1, it forces the block to start acquiring from the first sample
+     */
+    void set_state(int state);
+
+private:
+    ConfigurationInterface* configuration_;
+    pcps_acquisition_cc_sptr acquisition_cc_;
+    pcps_acquisition_sc_sptr acquisition_sc_;
+    gr::blocks::stream_to_vector::sptr stream_to_vector_;
+    gr::blocks::float_to_complex::sptr float_to_complex_;
+    complex_byte_to_float_x2_sptr cbyte_to_float_x2_;
+    size_t item_size_;
+    std::string item_type_;
+    unsigned int vector_length_;
+    unsigned int code_length_;
+    bool bit_transition_flag_;
+    bool use_CFAR_algorithm_flag_;
+    unsigned int channel_;
+    float threshold_;
+    unsigned int doppler_max_;
+    unsigned int doppler_step_;
+    unsigned int max_dwells_;
+    long fs_in_;
+    long if_;
+    bool dump_;
+    bool blocking_;
+    std::string dump_filename_;
+    std::complex<float> * code_;
+    Gnss_Synchro * gnss_synchro_;
+    std::string role_;
+    unsigned int in_streams_;
+    unsigned int out_streams_;
+
+    float calculate_threshold(float pfa);
+};
+
+#endif /* GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_ */

src/algorithms/libs/CMakeLists.txt

@@ -20,6 +20,7 @@ add_subdirectory(rtklib)
 
 set(GNSS_SPLIBS_SOURCES
 	gps_l2c_signal.cc
+	gps_l5_signal.cc
     galileo_e1_signal_processing.cc
     gnss_sdr_valve.cc
     gnss_sdr_sample_counter.cc

src/algorithms/libs/gps_l5_signal.cc

@@ -0,0 +1,304 @@
+/*!
+ * \file gps_l2c_signal.cc
+ * \brief This class implements signal generators for the GPS L2C signals
+ * \author Javier Arribas, 2015. jarribas(at)cttc.es
+ *
+ * Detailed description of the file here if needed.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include <iostream>
+#include <deque>
+#include <cmath>
+#include <cinttypes>
+#include <complex>
+
+#include "GPS_L5.h"
+
+using namespace std;
+
+std::deque<bool> l5i_xa_shift(std::deque<bool> xa)
+{
+  if (xa==std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
+    {
+      return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
+    }else{
+        std::deque<bool> out(xa.begin(), xa.end()-1);
+        out.push_front(xa[12] xor xa[11] xor xa[9] xor xa[8]);
+        return out;
+    }
+}
+
+std::deque<bool> l5q_xa_shift(std::deque<bool> xa)
+{
+  if (xa==std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
+    {
+      return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
+    }else{
+        std::deque<bool> out(xa.begin(), xa.end()-1);
+        out.push_front(xa[12] xor xa[11] xor xa[9] xor xa[8]);
+        return out;
+    }
+}
+
+std::deque<bool> l5i_xb_shift(std::deque<bool> xb)
+    {
+      std::deque<bool> out(xb.begin(), xb.end()-1);
+      out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]);
+      return out;
+    }
+
+std::deque<bool> l5q_xb_shift(std::deque<bool> xb)
+    {
+      std::deque<bool> out(xb.begin(), xb.end()-1);
+      out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]);
+      return out;
+    }
+
+std::deque<bool> make_l5i_xa()
+{
+  std::deque<bool> xa={1,1,1,1,1,1,1,1,1,1,1,1,1};
+  std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS,0);
+
+  for (int i=0; i<GPS_L5i_CODE_LENGTH_CHIPS; i++)
+    {
+      y[i] = xa[12];
+      xa = l5i_xa_shift(xa);
+    }
+  return y;
+}
+
+std::deque<bool> make_l5i_xb()
+{
+  std::deque<bool> xb={1,1,1,1,1,1,1,1,1,1,1,1,1};
+  std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS,0);
+
+  for (int i=0; i<GPS_L5i_CODE_LENGTH_CHIPS; i++)
+    {
+      y[i] = xb[12];
+      xb = l5i_xb_shift(xb);
+    }
+  return y;
+}
+
+std::deque<bool> make_l5q_xa()
+{
+  std::deque<bool> xa={1,1,1,1,1,1,1,1,1,1,1,1,1};
+  std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS,0);
+
+  for (int i=0; i<GPS_L5q_CODE_LENGTH_CHIPS; i++)
+    {
+      y[i] = xa[12];
+      xa = l5q_xa_shift(xa);
+    }
+  return y;
+}
+
+std::deque<bool> make_l5q_xb()
+{
+  std::deque<bool> xb={1,1,1,1,1,1,1,1,1,1,1,1,1};
+  std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS,0);
+
+  for (int i=0; i<GPS_L5q_CODE_LENGTH_CHIPS; i++)
+    {
+      y[i] = xb[12];
+      xb = l5q_xb_shift(xb);
+    }
+  return y;
+}
+
+void make_l5i(int32_t * _dest, int prn)
+{
+  int xb_offset = GPS_L5i_INIT_REG[prn];
+
+  std::deque<bool> xb=make_l5i_xb();
+  std::deque<bool> xa=make_l5i_xa();
+  std::deque<bool> xb_shift(GPS_L5i_CODE_LENGTH_CHIPS,0);
+
+  for (int n=0; n<GPS_L5i_CODE_LENGTH_CHIPS; n++)
+    {
+      xb_shift[n]=xb[(xb_offset+n)%GPS_L5i_CODE_LENGTH_CHIPS];
+    }
+  std::deque<bool> out_code(GPS_L5i_CODE_LENGTH_CHIPS,0);
+  for (int n=0; n<GPS_L5i_CODE_LENGTH_CHIPS; n++)
+    {
+      _dest[n]=xa[n] xor xb_shift[n];
+    }
+}
+
+void make_l5q(int32_t * _dest, int prn)
+{
+  int xb_offset = GPS_L5q_INIT_REG[prn];
+
+  std::deque<bool> xb=make_l5q_xb();
+  std::deque<bool> xa=make_l5q_xa();
+  std::deque<bool> xb_shift(GPS_L5q_CODE_LENGTH_CHIPS,0);
+
+  for (int n=0; n<GPS_L5q_CODE_LENGTH_CHIPS; n++)
+    {
+      xb_shift[n]=xb[(xb_offset+n)%GPS_L5q_CODE_LENGTH_CHIPS];
+    }
+  std::deque<bool> out_code(GPS_L5q_CODE_LENGTH_CHIPS,0);
+  for (int n=0; n<GPS_L5q_CODE_LENGTH_CHIPS; n++)
+    {
+      _dest[n]=xa[n] xor xb_shift[n];
+    }
+}
+
+void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
+{
+    int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
+
+    if (_prn > 0 and _prn < 51)
+        {
+          make_l5i(_code, _prn);
+        }
+
+    for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
+        {
+            _dest[i] = std::complex<float>(1.0 - 2.0 * _code[i], 0.0);
+        }
+
+    delete[] _code;
+}
+
+
+/*
+ *  Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency
+ */
+void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs)
+{
+    int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
+    if (_prn > 0 and _prn < 51)
+        {
+          make_l5i(_code, _prn);
+        }
+
+    signed int _samplesPerCode, _codeValueIndex;
+    float _ts;
+    float _tc;
+    const signed int _codeLength = GPS_L5i_CODE_LENGTH_CHIPS;
+
+    //--- Find number of samples per spreading code ----------------------------
+    _samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5i_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
+
+    //--- Find time constants --------------------------------------------------
+    _ts = 1.0 / static_cast<float>(_fs);   // Sampling period in sec
+    _tc = 1.0 / static_cast<float>(GPS_L5i_CODE_RATE_HZ);  // C/A chip period in sec
+
+    //float aux;
+    for (signed int i = 0; i < _samplesPerCode; i++)
+        {
+            //=== Digitizing =======================================================
+
+            //--- Make index array to read L5 code values -------------------------
+            //TODO: Check this formula! Seems to start with an extra sample
+            _codeValueIndex = ceil((_ts * (static_cast<float>(i) + 1)) / _tc) - 1;
+            //aux = (_ts * (i + 1)) / _tc;
+            //_codeValueIndex = static_cast<int>(static_cast<long>(aux)) - 1;
+
+            //--- Make the digitized version of the L2C code -----------------------
+            if (i == _samplesPerCode - 1)
+                {
+                    //--- Correct the last index (due to number rounding issues) -----------
+                    _dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeLength - 1], 0);
+                }
+            else
+                {
+                    _dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
+                }
+        }
+    delete[] _code;
+}
+
+
+void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
+{
+    int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
+
+    if (_prn > 0 and _prn < 51)
+        {
+          make_l5q(_code, _prn);
+        }
+
+    for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
+        {
+            _dest[i] = std::complex<float>(1.0 - 2.0 * _code[i], 0.0);
+        }
+
+    delete[] _code;
+}
+
+
+/*
+ *  Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency
+ */
+void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs)
+{
+    int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
+    if (_prn > 0 and _prn < 51)
+        {
+          make_l5q(_code, _prn);
+        }
+
+    signed int _samplesPerCode, _codeValueIndex;
+    float _ts;
+    float _tc;
+    const signed int _codeLength = GPS_L5q_CODE_LENGTH_CHIPS;
+
+    //--- Find number of samples per spreading code ----------------------------
+    _samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5q_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
+
+    //--- Find time constants --------------------------------------------------
+    _ts = 1.0 / static_cast<float>(_fs);   // Sampling period in sec
+    _tc = 1.0 / static_cast<float>(GPS_L5q_CODE_RATE_HZ);  // C/A chip period in sec
+
+    //float aux;
+    for (signed int i = 0; i < _samplesPerCode; i++)
+        {
+            //=== Digitizing =======================================================
+
+            //--- Make index array to read L5 code values -------------------------
+            //TODO: Check this formula! Seems to start with an extra sample
+            _codeValueIndex = ceil((_ts * (static_cast<float>(i) + 1)) / _tc) - 1;
+            //aux = (_ts * (i + 1)) / _tc;
+            //_codeValueIndex = static_cast<int>(static_cast<long>(aux)) - 1;
+
+            //--- Make the digitized version of the L2C code -----------------------
+            if (i == _samplesPerCode - 1)
+                {
+                    //--- Correct the last index (due to number rounding issues) -----------
+                    _dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeLength - 1], 0);
+                }
+            else
+                {
+                    _dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
+                }
+        }
+    delete[] _code;
+}
+
+

src/algorithms/libs/gps_l5_signal.h

@@ -0,0 +1,52 @@
+/*!
+ * \file gps_l5_signal.h
+ * \brief This class implements signal generators for the GPS L2C signals
+ * \author Javier Arribas, 2017. jarribas(at)cttc.es
+ *
+ * Detailed description of the file here if needed.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_GPS_L5_SIGNAL_H_
+#define GNSS_GPS_L5_SIGNAL_H_
+
+#include <complex>
+
+
+//!Generates complex GPS L5i M code for the desired SV ID
+void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn);
+
+//!Generates complex GPS L5q M code for the desired SV ID
+void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn);
+
+//! Generates complex GPS L5i M code for the desired SV ID, and sampled to specific sampling frequency
+void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs);
+
+//! Generates complex GPS L5q M code for the desired SV ID, and sampled to specific sampling frequency
+void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs);
+
+
+#endif /* GNSS_GPS_L5_SIGNAL_H_ */

src/algorithms/tracking/adapters/CMakeLists.txt

@@ -33,6 +33,7 @@ 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_l5i_dll_pll_tracking.cc
      ${OPT_TRACKING_ADAPTERS}
 )
  

src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc

@@ -0,0 +1,145 @@
+/*!
+ * \file gps_l5idll_pll_tracking.cc
+ * \brief  Interface of an adapter of a DLL+PLL tracking loop block
+ * for GPS L5i to a TrackingInterface
+ * \author Javier Arribas, 2017. 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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+
+#include "gps_l5i_dll_pll_tracking.h"
+#include <glog/logging.h>
+#include "GPS_L5.h"
+#include "configuration_interface.h"
+
+
+using google::LogMessage;
+
+GpsL5iDllPllTracking::GpsL5iDllPllTracking(
+        ConfigurationInterface* configuration, std::string role,
+        unsigned int in_streams, unsigned int out_streams) :
+                role_(role), in_streams_(in_streams), out_streams_(out_streams)
+{
+    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);
+    int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
+    fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
+    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(static_cast<double>(fs_in) / (static_cast<double>(GPS_L5i_CODE_RATE_HZ) / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
+
+    //################# MAKE TRACKING GNURadio object ###################
+    if (item_type.compare("gr_complex") == 0)
+        {
+            item_size_ = sizeof(gr_complex);
+            tracking_ = gps_l5i_dll_pll_make_tracking_cc(
+                    f_if,
+                    fs_in,
+                    vector_length,
+                    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;
+    DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
+}
+
+
+GpsL5iDllPllTracking::~GpsL5iDllPllTracking()
+{}
+
+
+void GpsL5iDllPllTracking::start_tracking()
+{
+    tracking_->start_tracking();
+}
+
+/*
+ * Set tracking channel unique ID
+ */
+void GpsL5iDllPllTracking::set_channel(unsigned int channel)
+{
+    channel_ = channel;
+    tracking_->set_channel(channel);
+}
+
+
+void GpsL5iDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
+{
+    tracking_->set_gnss_synchro(p_gnss_synchro);
+}
+
+void GpsL5iDllPllTracking::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 GpsL5iDllPllTracking::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 GpsL5iDllPllTracking::get_left_block()
+{
+    return tracking_;
+}
+
+gr::basic_block_sptr GpsL5iDllPllTracking::get_right_block()
+{
+    return tracking_;
+}
+

src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h

@@ -0,0 +1,103 @@
+/*!
+ * \file gps_l5idll_pll_tracking.h
+ * \brief  Interface of an adapter of a DLL+PLL tracking loop block
+ * for GPS L5i to a TrackingInterface
+ * \author Javier Arribas, 2017. 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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_gps_l5i_dll_pll_tracking_H_
+#define GNSS_SDR_gps_l5i_dll_pll_tracking_H_
+
+#include <string>
+#include "tracking_interface.h"
+#include "gps_l5i_dll_pll_tracking_cc.h"
+
+
+class ConfigurationInterface;
+
+/*!
+ * \brief This class implements a code DLL + carrier PLL tracking loop
+ */
+class GpsL5iDllPllTracking : public TrackingInterface
+{
+public:
+    GpsL5iDllPllTracking(ConfigurationInterface* configuration,
+            std::string role,
+            unsigned int in_streams,
+            unsigned int out_streams);
+
+    virtual ~GpsL5iDllPllTracking();
+
+    inline std::string role() override
+    {
+        return role_;
+    }
+
+    //! Returns "GPS_L2_M_DLL_PLL_Tracking"
+    inline std::string implementation() override
+    {
+        return "GPS_L5i_DLL_PLL_Tracking";
+    }
+
+    inline size_t item_size() override
+    {
+        return item_size_;
+    }
+
+    void connect(gr::top_block_sptr top_block) override;
+    void disconnect(gr::top_block_sptr top_block) override;
+    gr::basic_block_sptr get_left_block() override;
+    gr::basic_block_sptr get_right_block() override;
+
+    /*!
+     * \brief Set tracking channel unique ID
+     */
+    void set_channel(unsigned int channel) override;
+
+    /*!
+     * \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) override;
+
+    void start_tracking() override;
+
+private:
+    gps_l5i_dll_pll_tracking_cc_sptr tracking_;
+    size_t item_size_;
+    unsigned int channel_;
+    std::string role_;
+    unsigned int in_streams_;
+    unsigned int out_streams_;
+};
+
+#endif // GNSS_SDR_gps_l5i_dll_pll_tracking_H_

src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt

@@ -33,6 +33,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
      gps_l1_ca_tcp_connector_tracking_cc.cc
      galileo_e5a_dll_pll_tracking_cc.cc
      gps_l2_m_dll_pll_tracking_cc.cc
+     gps_l5i_dll_pll_tracking_cc.cc
      gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
      gps_l1_ca_dll_pll_c_aid_tracking_sc.cc     
      ${OPT_TRACKING_BLOCKS}   

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -534,6 +534,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
     // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
     Gnss_Synchro current_synchro_data = Gnss_Synchro();
 
+    int next_prn_lenght_samples = d_current_prn_length_samples;
     if (d_enable_tracking == true)
         {
             // Fill the acquisition data
@@ -570,6 +571,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
                     d_code_phase_step_chips,
                     d_current_prn_length_samples);
 
+
             // ################## PLL ##########################################################
             // PLL discriminator
             // Update PLL discriminator [rads/Ti -> Secs/Ti]
@@ -598,7 +600,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
             //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
             double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
             double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
-            d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
+            next_prn_lenght_samples = round(K_blk_samples); // round to a discrete number of samples
+            //d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
 
             //################### PLL COMMANDS #################################################
             // carrier phase step (NCO phase increment per sample) [rads/sample]
@@ -613,7 +616,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
             // code phase step (Code resampler phase increment per sample) [chips/sample]
             d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
             // remnant code phase [chips]
-            d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
+            //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
+            d_rem_code_phase_samples = K_blk_samples - next_prn_lenght_samples; // rounding error < 1 sample
             d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
 
             // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
@@ -736,6 +740,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
 
     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
+
+    d_current_prn_length_samples=next_prn_lenght_samples;
     return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false
 }
 

src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc

@@ -0,0 +1,765 @@
+/*!
+ * \file gps_l5i_dll_pll_tracking_cc.cc
+ * \brief Implementation of a code DLL + carrier PLL tracking block for GPS L2C
+ * \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:
+ * [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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "gps_l5i_dll_pll_tracking_cc.h"
+#include <cmath>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <boost/lexical_cast.hpp>
+#include <gnuradio/io_signature.h>
+#include <glog/logging.h>
+#include <matio.h>
+#include <volk_gnsssdr/volk_gnsssdr.h>
+#include "gps_l5_signal.h"
+#include "tracking_discriminators.h"
+#include "lock_detectors.h"
+#include "GPS_L5.h"
+#include "control_message_factory.h"
+
+
+/*!
+ * \todo Include in definition header file
+ */
+#define GPS_L2M_CN0_ESTIMATION_SAMPLES 10
+#define GPS_L2M_MINIMUM_VALID_CN0 25
+#define GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER 50
+#define GPS_L2M_CARRIER_LOCK_THRESHOLD 0.75
+
+
+using google::LogMessage;
+
+gps_l5i_dll_pll_tracking_cc_sptr
+gps_l5i_dll_pll_make_tracking_cc(
+        long if_freq,
+        long fs_in,
+        unsigned int vector_length,
+        bool dump,
+        std::string dump_filename,
+        float pll_bw_hz,
+        float dll_bw_hz,
+        float early_late_space_chips)
+{
+    return gps_l5i_dll_pll_tracking_cc_sptr(new gps_l5i_dll_pll_tracking_cc(if_freq,
+            fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
+}
+
+
+void gps_l5i_dll_pll_tracking_cc::forecast (int noutput_items,
+        gr_vector_int &ninput_items_required)
+{
+    if (noutput_items != 0)
+        {
+            ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; //set the required available samples in each call
+        }
+}
+
+
+gps_l5i_dll_pll_tracking_cc::gps_l5i_dll_pll_tracking_cc(
+        long if_freq,
+        long fs_in,
+        unsigned int vector_length,
+        bool dump,
+        std::string dump_filename,
+        float pll_bw_hz,
+        float dll_bw_hz,
+        float early_late_space_chips) :
+                gr::block("gps_l5i_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
+                        gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
+{
+    // Telemetry bit synchronization message port input
+    this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
+    this->message_port_register_out(pmt::mp("events"));
+    // initialize internal vars
+    d_dump = dump;
+    d_if_freq = if_freq;
+    d_fs_in = fs_in;
+    d_vector_length = vector_length;
+    d_dump_filename = dump_filename;
+
+    d_current_prn_length_samples = static_cast<int>(d_vector_length);
+
+    // DLL/PLL filter initialization
+    d_carrier_loop_filter=Tracking_2nd_PLL_filter(GPS_L5i_PERIOD);
+    d_code_loop_filter=Tracking_2nd_DLL_filter(GPS_L5i_PERIOD);
+
+    // 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)
+
+    // Initialization of local code replica
+    // Get space for a vector with the C/A code replica sampled 1x/chip
+    d_ca_code = static_cast<gr_complex*>(volk_gnsssdr_malloc(static_cast<int>(GPS_L5i_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
+
+    // correlator outputs (scalar)
+    d_n_correlator_taps = 3; // Early, Prompt, and Late
+    d_correlator_outs = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment()));
+    for (int n = 0; n < d_n_correlator_taps; n++)
+        {
+            d_correlator_outs[n] = gr_complex(0,0);
+        }
+    d_local_code_shift_chips = static_cast<float*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment()));
+    // Set TAPs delay values [chips]
+    d_local_code_shift_chips[0] = - d_early_late_spc_chips;
+    d_local_code_shift_chips[1] = 0.0;
+    d_local_code_shift_chips[2] = d_early_late_spc_chips;
+
+    multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps);
+
+    //--- Perform initializations ------------------------------
+    // define initial code frequency basis of NCO
+    d_code_freq_chips = GPS_L5i_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;
+
+    // CN0 estimation and lock detector buffers
+    d_cn0_estimation_counter = 0;
+    d_Prompt_buffer = new gr_complex[GPS_L2M_CN0_ESTIMATION_SAMPLES];
+    d_carrier_lock_test = 1;
+    d_CN0_SNV_dB_Hz = 0;
+    d_carrier_lock_fail_counter = 0;
+    d_carrier_lock_threshold = GPS_L2M_CARRIER_LOCK_THRESHOLD;
+
+    systemName["G"] = std::string("GPS");
+
+    //set_min_output_buffer((long int)300);
+
+    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_rem_code_phase_chips = 0.0;
+    d_code_phase_step_chips = 0.0;
+    d_carrier_phase_step_rad = 0.0;
+
+    set_relative_rate(1.0 / static_cast<double>(d_vector_length));
+}
+
+
+void gps_l5i_dll_pll_tracking_cc::start_tracking()
+{
+    /*
+     *  correct the code phase according to the delay between acq and trk
+     */
+    d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
+    d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
+    d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
+
+    long int acq_trk_diff_samples;
+    double acq_trk_diff_seconds;
+    acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(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<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
+    // Doppler effect
+    // Fd=(C/(C+Vr))*F
+    double radial_velocity = (GPS_L5_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L5_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_L5i_CODE_RATE_HZ;
+    d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
+    T_chip_mod_seconds = 1/d_code_freq_chips;
+    T_prn_mod_seconds = T_chip_mod_seconds * GPS_L5i_CODE_LENGTH_CHIPS;
+    T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
+
+    d_current_prn_length_samples = round(T_prn_mod_samples);
+
+    double T_prn_true_seconds = GPS_L5i_CODE_LENGTH_CHIPS / GPS_L5i_CODE_RATE_HZ;
+    double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(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<double>(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_L5_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
+
+    // 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_l5i_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN);
+
+    multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L5i_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
+    for (int n = 0; n < d_n_correlator_taps; n++)
+        {
+            d_correlator_outs[n] = gr_complex(0,0);
+        }
+
+    d_carrier_lock_fail_counter = 0;
+    d_rem_code_phase_samples = 0;
+    d_rem_carr_phase_rad = 0.0;
+    d_rem_code_phase_chips = 0.0;
+    d_acc_carrier_phase_rad = 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 of GPS L2CM signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
+    LOG(INFO) << "Starting GPS L2CM 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) << "GPS L2CM 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;
+}
+
+
+int gps_l5i_dll_pll_tracking_cc::save_matfile()
+{
+    // READ DUMP FILE
+    std::ifstream::pos_type size;
+    int number_of_double_vars = 11;
+    int number_of_float_vars = 5;
+    int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars +
+            sizeof(float) * number_of_float_vars + sizeof(unsigned int);
+    std::ifstream dump_file;
+    dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
+    try
+    {
+            dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
+    }
+    catch(const std::ifstream::failure &e)
+    {
+            std::cerr << "Problem opening dump file:" <<  e.what() << std::endl;
+            return 1;
+    }
+    // count number of epochs and rewind
+    long int num_epoch = 0;
+    if (dump_file.is_open())
+        {
+            size = dump_file.tellg();
+            num_epoch = static_cast<long int>(size) / static_cast<long int>(epoch_size_bytes);
+            dump_file.seekg(0, std::ios::beg);
+        }
+    else
+        {
+            return 1;
+        }
+    float * abs_E = new float [num_epoch];
+    float * abs_P = new float [num_epoch];
+    float * abs_L = new float [num_epoch];
+    float * Prompt_I = new float [num_epoch];
+    float * Prompt_Q = new float [num_epoch];
+    unsigned long int * PRN_start_sample_count = new unsigned long int [num_epoch];
+    double * acc_carrier_phase_rad = new double [num_epoch];
+    double * carrier_doppler_hz = new double [num_epoch];
+    double * code_freq_chips = new double [num_epoch];
+    double * carr_error_hz = new double [num_epoch];
+    double * carr_error_filt_hz = new double [num_epoch];
+    double * code_error_chips = new double [num_epoch];
+    double * code_error_filt_chips = new double [num_epoch];
+    double * CN0_SNV_dB_Hz = new double [num_epoch];
+    double * carrier_lock_test = new double [num_epoch];
+    double * aux1 = new double [num_epoch];
+    double * aux2 = new double [num_epoch];
+    unsigned int * PRN = new unsigned int [num_epoch];
+
+    try
+    {
+            if (dump_file.is_open())
+                {
+                    for(long int i = 0; i < num_epoch; i++)
+                        {
+                            dump_file.read(reinterpret_cast<char *>(&abs_E[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_P[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&abs_L[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&Prompt_I[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&Prompt_Q[i]), sizeof(float));
+                            dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(unsigned long int));
+                            dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&CN0_SNV_dB_Hz[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&carrier_lock_test[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&aux1[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&aux2[i]), sizeof(double));
+                            dump_file.read(reinterpret_cast<char *>(&PRN[i]), sizeof(unsigned int));
+                        }
+                }
+            dump_file.close();
+    }
+    catch (const std::ifstream::failure &e)
+    {
+            std::cerr << "Problem reading dump file:" <<  e.what() << std::endl;
+            delete[] abs_E;
+            delete[] abs_P;
+            delete[] abs_L;
+            delete[] Prompt_I;
+            delete[] Prompt_Q;
+            delete[] PRN_start_sample_count;
+            delete[] acc_carrier_phase_rad;
+            delete[] carrier_doppler_hz;
+            delete[] code_freq_chips;
+            delete[] carr_error_hz;
+            delete[] carr_error_filt_hz;
+            delete[] code_error_chips;
+            delete[] code_error_filt_chips;
+            delete[] CN0_SNV_dB_Hz;
+            delete[] carrier_lock_test;
+            delete[] aux1;
+            delete[] aux2;
+            delete[] PRN;
+            return 1;
+    }
+
+    // WRITE MAT FILE
+    mat_t *matfp;
+    matvar_t *matvar;
+    std::string filename = d_dump_filename;
+    filename.erase(filename.length() - 4, 4);
+    filename.append(".mat");
+    matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
+    if(reinterpret_cast<long*>(matfp) != NULL)
+        {
+            size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
+            matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+
+            matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
+            Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+            Mat_VarFree(matvar);
+        }
+    Mat_Close(matfp);
+    delete[] abs_E;
+    delete[] abs_P;
+    delete[] abs_L;
+    delete[] Prompt_I;
+    delete[] Prompt_Q;
+    delete[] PRN_start_sample_count;
+    delete[] acc_carrier_phase_rad;
+    delete[] carrier_doppler_hz;
+    delete[] code_freq_chips;
+    delete[] carr_error_hz;
+    delete[] carr_error_filt_hz;
+    delete[] code_error_chips;
+    delete[] code_error_filt_chips;
+    delete[] CN0_SNV_dB_Hz;
+    delete[] carrier_lock_test;
+    delete[] aux1;
+    delete[] aux2;
+    delete[] PRN;
+    return 0;
+}
+
+
+gps_l5i_dll_pll_tracking_cc::~gps_l5i_dll_pll_tracking_cc()
+{
+    if (d_dump_file.is_open())
+        {
+            try
+            {
+                    d_dump_file.close();
+            }
+            catch(const std::exception & ex)
+            {
+                    LOG(WARNING) << "Exception in destructor " << ex.what();
+            }
+        }
+    if(d_dump)
+        {
+            if(d_channel == 0)
+                {
+                    std::cout << "Writing .mat files ...";
+                }
+            gps_l5i_dll_pll_tracking_cc::save_matfile();
+            if(d_channel == 0)
+                {
+                    std::cout << " done." << std::endl;
+                }
+        }
+    try
+    {
+            volk_gnsssdr_free(d_local_code_shift_chips);
+            volk_gnsssdr_free(d_correlator_outs);
+            volk_gnsssdr_free(d_ca_code);
+            delete[] d_Prompt_buffer;
+            multicorrelator_cpu.free();
+    }
+    catch(const std::exception & ex)
+    {
+            LOG(WARNING) << "Exception in destructor " << ex.what();
+    }
+}
+
+
+int gps_l5i_dll_pll_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
+        gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
+{
+    // process vars
+    double carr_error_hz = 0;
+    double carr_error_filt_hz = 0;
+    double code_error_chips = 0;
+    double code_error_filt_chips = 0;
+
+    // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
+    Gnss_Synchro current_synchro_data = Gnss_Synchro();
+
+    // Block input data and block output stream pointers
+    const gr_complex* in = reinterpret_cast<const gr_complex *>(input_items[0]);
+    Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
+
+    if (d_enable_tracking == true)
+        {
+            // Fill the acquisition data
+            current_synchro_data = *d_acquisition_gnss_synchro;
+            // Receiver signal alignment
+            if (d_pull_in == true)
+                {
+                    int samples_offset;
+                    double acq_trk_shif_correction_samples;
+                    int acq_to_trk_delay_samples;
+                    acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
+                    acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
+                    samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
+                    current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset;
+                    d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples
+                    d_pull_in = false;
+                    // take into account the carrier cycles accumulated in the pull in signal alignment
+                    d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset;
+                    current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+                    current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+                    current_synchro_data.fs = d_fs_in;
+                    current_synchro_data.correlation_length_ms = 20;
+                    *out[0] = current_synchro_data;
+                    consume_each(samples_offset); // shift input to perform alignment with local replica
+                    return 1;
+                }
+
+            // ################# CARRIER WIPEOFF AND CORRELATORS ##############################
+            // perform carrier wipe-off and compute Early, Prompt and Late correlation
+            multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
+            multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
+                    d_carrier_phase_step_rad,
+                    d_rem_code_phase_chips,
+                    d_code_phase_step_chips,
+                    d_current_prn_length_samples);
+
+            // ################## PLL ##########################################################
+            // PLL discriminator
+            // Update PLL discriminator [rads/Ti -> Secs/Ti]
+            carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_L5_TWO_PI;
+            // Carrier discriminator filter
+            carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
+            // New carrier Doppler frequency estimation
+            d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
+            // New code Doppler frequency estimation
+            d_code_freq_chips = GPS_L5i_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L5i_CODE_RATE_HZ) / GPS_L5_FREQ_HZ);
+
+            // ################## DLL ##########################################################
+            // DLL discriminator
+            code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti]
+            // Code discriminator filter
+            code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
+            double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
+            double T_prn_seconds = T_chip_seconds * GPS_L5i_CODE_LENGTH_CHIPS;
+            double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds]
+            //double code_error_filt_secs = (GPS_L5i_PERIOD * code_error_filt_chips) / GPS_L5i_CODE_RATE_HZ; //[seconds]
+
+            // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
+            // keep alignment parameters for the next input buffer
+            // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
+            double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
+            double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
+            d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
+
+            //################### PLL COMMANDS #################################################
+            // carrier phase step (NCO phase increment per sample) [rads/sample]
+            d_carrier_phase_step_rad = GPS_L5_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
+            // remnant carrier phase to prevent overflow in the code NCO
+            d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples;
+            d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L5_TWO_PI);
+            // carrier phase accumulator
+            d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples;
+
+            //################### DLL COMMANDS #################################################
+            // code phase step (Code resampler phase increment per sample) [chips/sample]
+            d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
+            // remnant code phase [chips]
+            d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
+            d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
+
+            // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
+            if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES)
+                {
+                    // fill buffer with prompt correlator output values
+                    d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1];
+                    d_cn0_estimation_counter++;
+                }
+            else
+                {
+                    d_cn0_estimation_counter = 0;
+                    // Code lock indicator
+                    d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L5i_CODE_LENGTH_CHIPS);
+                    // Carrier lock indicator
+                    d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES);
+                    // Loss of lock detection
+                    if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < GPS_L2M_MINIMUM_VALID_CN0)
+                        {
+                            d_carrier_lock_fail_counter++;
+                        }
+                    else
+                        {
+                            if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
+                        }
+                    if (d_carrier_lock_fail_counter > GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER)
+                        {
+                            std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
+                            LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
+                            this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock
+                            d_carrier_lock_fail_counter = 0;
+                            d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
+                        }
+                }
+            // ########### Output the tracking data to navigation and PVT ##########
+            current_synchro_data.Prompt_I = static_cast<double>(d_correlator_outs[1].real());
+            current_synchro_data.Prompt_Q = static_cast<double>(d_correlator_outs[1].imag());
+            current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
+            current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
+            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_symbol_output = true;
+            current_synchro_data.correlation_length_ms = 20;
+        }
+    else
+        {
+            for (int n = 0; n < d_n_correlator_taps; n++)
+                {
+                    d_correlator_outs[n] = gr_complex(0,0);
+                }
+            current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
+            current_synchro_data.correlation_length_ms = 20;
+        }
+    //assign the GNURadio block output data
+    current_synchro_data.fs = d_fs_in;
+    *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[1].real();
+            prompt_Q = d_correlator_outs[1].imag();
+            tmp_E = std::abs<float>(d_correlator_outs[0]);
+            tmp_P = std::abs<float>(d_correlator_outs[1]);
+            tmp_L = std::abs<float>(d_correlator_outs[2]);
+            try
+            {
+                    // EPR
+                    d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
+                    // PROMPT I and Q (to analyze navigation symbols)
+                    d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
+                    d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
+                    // PRN start sample stamp
+                    //tmp_float=(float)d_sample_counter;
+                    d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
+                    // accumulated carrier phase
+                    d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
+
+                    // carrier and code frequency
+                    d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+                    d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
+
+                    //PLL commands
+                    d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
+                    d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+
+                    //DLL commands
+                    d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
+                    d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
+
+                    // CN0 and carrier lock test
+                    d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
+                    d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
+
+                    // AUX vars (for debug purposes)
+                    tmp_double = d_rem_code_phase_samples;
+                    d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+                    tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+                    d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+
+                    // PRN
+                    unsigned int prn_ = d_acquisition_gnss_synchro->PRN;
+                    d_dump_file.write(reinterpret_cast<char*>(&prn_), sizeof(unsigned int));
+            }
+            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
+    return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false
+}
+
+
+void gps_l5i_dll_pll_tracking_cc::set_channel(unsigned int channel)
+{
+    d_channel = channel;
+    LOG(INFO) << "Tracking Channel set to " << d_channel;
+    // ############# ENABLE DATA FILE LOG #################
+    if (d_dump == true)
+        {
+            if (d_dump_file.is_open() == false)
+                {
+                    try
+                    {
+                            d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
+                            d_dump_filename.append(".dat");
+                            d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
+                            d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
+                            LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
+                    }
+                    catch (std::ifstream::failure& e)
+                    {
+                            LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
+                    }
+                }
+        }
+}
+
+
+void gps_l5i_dll_pll_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
+{
+    d_acquisition_gnss_synchro = p_gnss_synchro;
+}

src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.h

@@ -0,0 +1,169 @@
+/*!
+ * \file gps_l5i_dll_pll_tracking_cc.h
+ * \brief Interface of a code DLL + carrier PLL tracking block for GPS L2C
+ * \author Javier Arribas, 2015. 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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H
+#define GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H
+
+#include <fstream>
+#include <map>
+#include <string>
+#include <gnuradio/block.h>
+#include "gnss_synchro.h"
+#include "tracking_2nd_DLL_filter.h"
+#include "tracking_2nd_PLL_filter.h"
+#include "cpu_multicorrelator.h"
+
+class gps_l5i_dll_pll_tracking_cc;
+
+typedef boost::shared_ptr<gps_l5i_dll_pll_tracking_cc>
+        gps_l5i_dll_pll_tracking_cc_sptr;
+
+gps_l5i_dll_pll_tracking_cc_sptr
+gps_l5i_dll_pll_make_tracking_cc(long if_freq,
+                                   long fs_in, unsigned
+                                   int vector_length,
+                                   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_l5i_dll_pll_tracking_cc: public gr::block
+{
+public:
+    ~gps_l5i_dll_pll_tracking_cc();
+
+    void set_channel(unsigned int channel);
+    void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
+    void start_tracking();
+
+    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_l5i_dll_pll_tracking_cc_sptr
+    gps_l5i_dll_pll_make_tracking_cc(long if_freq,
+            long fs_in, unsigned
+            int vector_length,
+            bool dump,
+            std::string dump_filename,
+            float pll_bw_hz,
+            float dll_bw_hz,
+            float early_late_space_chips);
+
+    gps_l5i_dll_pll_tracking_cc(long if_freq,
+            long fs_in, unsigned
+            int vector_length,
+            bool dump,
+            std::string dump_filename,
+            float pll_bw_hz,
+            float dll_bw_hz,
+            float early_late_space_chips);
+
+    // tracking configuration vars
+    unsigned int d_vector_length;
+    bool d_dump;
+
+    Gnss_Synchro* d_acquisition_gnss_synchro;
+    unsigned int d_channel;
+    long d_if_freq;
+    long d_fs_in;
+
+    double d_early_late_spc_chips;
+
+    // remaining code phase and carrier phase between tracking loops
+    double d_rem_code_phase_samples;
+    double d_rem_code_phase_chips;
+    double d_rem_carr_phase_rad;
+
+    // PLL and DLL filter library
+    Tracking_2nd_DLL_filter d_code_loop_filter;
+    Tracking_2nd_PLL_filter d_carrier_loop_filter;
+
+    // acquisition
+    double d_acq_code_phase_samples;
+    double d_acq_carrier_doppler_hz;
+    // correlator
+    int d_n_correlator_taps;
+    gr_complex* d_ca_code;
+    float* d_local_code_shift_chips;
+    gr_complex* d_correlator_outs;
+    cpu_multicorrelator multicorrelator_cpu;
+
+    // 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_rad;
+    double d_code_phase_samples;
+
+    // PRN period in samples
+    int d_current_prn_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<std::string, std::string> systemName;
+    std::string sys;
+
+    int save_matfile();
+};
+
+#endif //GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H

src/core/receiver/gnss_block_factory.cc

@@ -69,6 +69,7 @@
 #include "notch_filter_lite.h"
 #include "gps_l1_ca_pcps_acquisition.h"
 #include "gps_l2_m_pcps_acquisition.h"
+#include "gps_l5i_pcps_acquisition.h"
 #include "gps_l1_ca_pcps_tong_acquisition.h"
 #include "gps_l1_ca_pcps_assisted_acquisition.h"
 #include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
@@ -86,6 +87,7 @@
 #include "galileo_e1_tcp_connector_tracking.h"
 #include "galileo_e5a_dll_pll_tracking.h"
 #include "gps_l2_m_dll_pll_tracking.h"
+#include "gps_l5i_dll_pll_tracking.h"
 #include "gps_l1_ca_telemetry_decoder.h"
 #include "gps_l2c_telemetry_decoder.h"
 #include "galileo_e1b_telemetry_decoder.h"
@@ -981,6 +983,18 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
                     out_streams));
             block = std::move(block_);
         }
+    else if (implementation.compare("GPS_L2_M_PCPS_Acquisition") == 0)
+        {
+            std::unique_ptr<AcquisitionInterface> block_(new GpsL2MPcpsAcquisition(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
+    else if (implementation.compare("GPS_L5i_PCPS_Acquisition") == 0)
+        {
+            std::unique_ptr<AcquisitionInterface> block_(new GpsL5iPcpsAcquisition(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
     else if (implementation.compare("Galileo_E1_PCPS_Ambiguous_Acquisition") == 0)
         {
             std::unique_ptr<GNSSBlockInterface> block_(new GalileoE1PcpsAmbiguousAcquisition(configuration.get(), role, in_streams,
@@ -1054,6 +1068,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
                     out_streams));
             block = std::move(block_);
         }
+    else if (implementation.compare("GPS_L5i_DLL_PLL_Tracking") == 0)
+        {
+            std::unique_ptr<GNSSBlockInterface> block_(new GpsL5iDllPllTracking(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
 #if CUDA_GPU_ACCEL
     else if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking_GPU") == 0)
         {
@@ -1207,6 +1227,12 @@ std::unique_ptr<AcquisitionInterface> GNSSBlockFactory::GetAcqBlock(
                     out_streams));
             block = std::move(block_);
         }
+    else if (implementation.compare("GPS_L5i_PCPS_Acquisition") == 0)
+        {
+            std::unique_ptr<AcquisitionInterface> block_(new GpsL5iPcpsAcquisition(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
     else if (implementation.compare("Galileo_E1_PCPS_Ambiguous_Acquisition") == 0)
         {
             std::unique_ptr<AcquisitionInterface> block_(new GalileoE1PcpsAmbiguousAcquisition(configuration.get(), role, in_streams,
@@ -1312,6 +1338,12 @@ std::unique_ptr<TrackingInterface> GNSSBlockFactory::GetTrkBlock(
                     out_streams));
             block = std::move(block_);
         }
+    else if (implementation.compare("GPS_L5i_DLL_PLL_Tracking") == 0)
+        {
+            std::unique_ptr<TrackingInterface> block_(new GpsL5iDllPllTracking(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
 #if CUDA_GPU_ACCEL
     else if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking_GPU") == 0)
         {