GNSS-SDR Major changes:

New tracking libraries:
        -  tracking_discriminators: Library with a set of code tracking and carrier tracking discriminators that is used by the tracking algorithms. (fully documented, including math algorithms using doxygen!)
        -  tracking_2rd_DLL_filter: Class that implements 2 order DLL filter for code tracking loop.
        -  tracking_2rd_PLL_filter: Class that implements 2 order PLL filter for carrier tracking loop.
        -  tracking_FLL_PLL_filter:  Class that implements hybrid FLL and PLL filter for tracking carrier loop.
        -  CN_estimators: Library with a set of Carrier to Noise estimators and lock detectors. (fully documented, including math algorithms using doxygen!)

    Tracking:
        - gps_l1_ca_dll_pll_tracking: The existing DLL + PLL tracking module, which is the K.Borre and D.Akos one, is now completely re-factored. Now uses the above described libraries.
        - gps_l1_ca_dll_fll_pll_tracking: This is a brand new tracking module, which implements the FLL assisted PLL described in Kaplan (2nd edition). (also documentedwith references)

    Configuration options:
        - The following tracking parameters are added:
            ;######### TRACKING CONFIG ############
            ; Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking or GPS_L1_CA_DLL_FLL_PLL_Tracking 
            Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
            ;PLL filter bandwidth in Hz.
            Tracking.pll_bw_hz=50.0;
            ;DLL filter bandwidth in Hz.
            Tracking.dll_bw_hz=2.0;
            ;FLL filter bandwidth in Hz.
            Tracking.fll_bw_hz=50;
            ;filter order: choice between 2 or 3 at this moment, only for FLL assisted PLL
            Tracking.order=2;
            ;Correlator space in chips units
            Tracking.early_late_space_chips=0.5;

Other files have also been modified with minor changes to adapt to new modules or minor bug fixes.


git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@80 64b25241-fba3-4117-9849-534c7e92360d

conf/mercurio.conf

@@ -9,14 +9,14 @@ ControlThread.wait_for_flowgraph=false
 SignalSource.implementation=File_Signal_Source
 ;SignalSource.filename=/media/DATALOGGER/signals/spirent scenario 2/data/sc2_d8.dat
 ;SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
-SignalSource.filename=/home/luis/Project/signals/cap2/agilent_cap2.dat
+SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
 ;SignalSource.filename=/home/luis/Project/signals/GPS_L1_8sats_4_Msps_CN0_52.dat
 SignalSource.item_type=gr_complex
 SignalSource.sampling_frequency=4000000
-SignalSource.samples=0
+SignalSource.samples=292000000
 SignalSource.repeat=false
 SignalSource.dump=false
-SignalSource.enable_throttle_control=true
+SignalSource.enable_throttle_control=false
 
 ;######### SIGNAL_CONDITIONER CONFIG ############
 SignalConditioner.implementation=Pass_Through
@@ -26,7 +26,7 @@ SignalConditioner.sample_freq_out=4000000
 SignalConditioner.dump=false
 
 ;######### CHANNELS CONFIGURATION CONFIG ############
-Channels.count=4
+Channels.count=1
 
 ;######### ACQUISITION CONFIG ############
 
@@ -42,7 +42,7 @@ Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
 Acquisition0.threshold=440
 Acquisition0.doppler_max=10000
 Acquisition0.doppler_step=500
-Acquisition0.satellite=2
+Acquisition0.satellite=14
 Acquisition0.repeat_satellite=true
 
 ;######### ACQUISITION 1 CONFIG ############
@@ -58,7 +58,7 @@ Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
 Acquisition2.threshold=440
 Acquisition2.doppler_max=10000
 Acquisition2.doppler_step=500
-Acquisition2.satellite=11
+Acquisition2.satellite=1
 Acquisition2.repeat_satellite=true
 
 ;######### ACQUISITION 3 CONFIG ############
@@ -66,17 +66,22 @@ Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
 Acquisition3.threshold=440
 Acquisition3.doppler_max=10000
 Acquisition3.doppler_step=500
-Acquisition3.satellite=32
+Acquisition3.satellite=1
 Acquisition3.repeat_satellite=true
 	
 ;######### TRACKING CONFIG ############
-Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
+;Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
+Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
 Tracking.item_type=gr_complex
 Tracking.vector_length=4000
 Tracking.fs_in=4000000
 Tracking.if=0
 Tracking.dump=true
-Tracking.dump_filename=./trk_dump.dat
+Tracking.pll_bw_hz=50.0;
+Tracking.dll_bw_hz=2.0;
+Tracking.fll_bw_hz=50;
+Tracking.order=2;
+Tracking.early_late_space_chips=0.5;
 
 ;######### TELEMETRY DECODER CONFIG ############
 TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder

jamroot.jam

@@ -38,6 +38,7 @@ project : requirements
 <include>src/algorithms/telemetry_decoder/libs
 <include>src/algorithms/tracking/adapters
 <include>src/algorithms/tracking/gnuradio_blocks
+<include>src/algorithms/tracking/libs
 <include>src/core/interfaces
 <include>src/core/libs
 <include>src/core/receiver

src/algorithms/signal_source/adapters/file_signal_source.cc

@@ -234,16 +234,16 @@ gr_basic_block_sptr FileSignalSource::get_left_block()
 
 gr_basic_block_sptr FileSignalSource::get_right_block()
 {
-    if (dump_==true)
-    {
-    	return file_source_;
-    }else{
 	if (samples_ != 0)
 	{
 		return valve_;
 	}else
+	{
+		if (enable_throttle_control_ == true)
 		{
 			return throttle_;
+		}else{
+			return file_source_;
 		}
 	}
 }

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc

@@ -125,7 +125,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
       {
       d_GPS_FSM.Event_gps_word_preamble();
       d_preamble_index=d_sample_counter;//record the preamble sample stamp
-      std::cout<<"Pre-detection SAT "<<this->d_satellite+1<<std::endl;
+      std::cout<<"Pre-detection SAT "<<this->d_satellite<<std::endl;
       d_symbol_accumulator=0; //sync the symbol to bits integrator
       d_symbol_accumulator_counter=0;
       d_frame_bit_index=8;
@@ -141,12 +141,12 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
 
           if (!d_flag_frame_sync){
             d_flag_frame_sync=true;
-            std::cout<<" Frame sync SAT "<<this->d_satellite+1<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
+            std::cout<<" Frame sync SAT "<<this->d_satellite<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
           }
           }else
             {
             if (preamble_diff>7000){
-              std::cout<<"lost of frame sync SAT "<<this->d_satellite+1<<std::endl;
+              std::cout<<"lost of frame sync SAT "<<this->d_satellite<<std::endl;
               d_stat=0; //lost of frame sync
               d_flag_frame_sync=false;
             }
@@ -217,7 +217,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
     //gps_synchro.preamble_delay_ms=(float)d_preamble_index;
     gps_synchro.preamble_delay_ms=(float)d_preamble_index;
     gps_synchro.prn_delay_ms=in[3][0];
-    gps_synchro.satellite_PRN=d_satellite+1;
+    gps_synchro.satellite_PRN=d_satellite;
     gps_synchro.channel_ID=d_channel;
     *out[0]=gps_synchro;
     return 1;

src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc

@@ -147,7 +147,7 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
 
   subframe_ID=d_nav.subframe_decoder(this->d_subframe); //decode the subframe
 
-  d_nav.d_satellite_PRN=d_satellite_PRN+1;
+  d_nav.d_satellite_PRN=d_satellite_PRN;
   d_nav.d_channel_ID=d_channel_ID;
   if (subframe_ID==1) {
     d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms-6002;

src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc

@@ -0,0 +1,167 @@
+/*!
+ * \file gps_l1_ca_dll_fll_pll_tracking.cc
+ * \brief code DLL + carrier FLL/PLL tracking
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
+ * according to the algorithms described in [1]
+ * [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
+ * Applications, Second Edition, Artech House Publishers, 2005.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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_l1_ca_dll_fll_pll_tracking.h"
+
+#include "configuration_interface.h"
+
+#include <gnuradio/gr_io_signature.h>
+
+#include <glog/log_severity.h>
+#include <glog/logging.h>
+
+using google::LogMessage;
+
+GpsL1CaDllFllPllTracking::GpsL1CaDllFllPllTracking(
+        ConfigurationInterface* configuration, std::string role,
+        unsigned int in_streams, unsigned int out_streams,
+        gr_msg_queue_sptr queue) :
+    role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(
+            queue)
+{
+
+    DLOG(INFO) << "role " << role;
+    //DLOG(INFO) << "vector length " << vector_length;
+
+    //################# CONFIGURATION PARAMETERS ########################
+
+    int fs_in;
+    int vector_length;
+    int f_if;
+    bool dump;
+    std::string dump_filename;
+    std::string item_type;
+    std::string default_item_type = "gr_complex";
+    float pll_bw_hz;
+    float fll_bw_hz;
+    float dll_bw_hz;
+    float early_late_space_chips;
+    int order;
+
+    item_type = configuration->property(role + ".item_type",default_item_type);
+    vector_length = configuration->property(role + ".vector_length", 2048);
+    fs_in = configuration->property(role + ".fs_in", 2048000);
+    f_if = configuration->property(role + ".if", 0);
+    dump = configuration->property(role + ".dump", false);
+    order = configuration->property(role + ".order", 2);
+    pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
+    fll_bw_hz = configuration->property(role + ".fll_bw_hz", 100.0);
+    dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
+    early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
+
+    std::string default_dump_filename = "./tracking.dat";
+    dump_filename = configuration->property(role + ".dump_filename",
+            default_dump_filename); //unused!
+
+    //################# MAKE TRACKING GNURadio object ###################
+    if (item_type.compare("gr_complex") == 0)
+    {
+        item_size_ = sizeof(gr_complex);
+        tracking_ = gps_l1_ca_dll_fll_pll_make_tracking_cc(satellite_, f_if,
+                fs_in, vector_length, queue_, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips);
+    }
+    else
+    {
+        LOG_AT_LEVEL(WARNING) << item_type << " unknown tracking item type.";
+    }
+
+    DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
+}
+
+GpsL1CaDllFllPllTracking::~GpsL1CaDllFllPllTracking()
+{
+}
+
+void GpsL1CaDllFllPllTracking::start_tracking()
+{
+    tracking_->start_tracking();
+}
+
+void GpsL1CaDllFllPllTracking::set_satellite(unsigned int satellite)
+{
+    satellite_ = satellite;
+    tracking_->set_satellite(satellite);
+    DLOG(INFO) << "satellite set to " << satellite_;
+}
+
+void GpsL1CaDllFllPllTracking::set_channel(unsigned int channel)
+{
+    channel_ = channel;
+    tracking_->set_channel(channel);
+}
+
+void GpsL1CaDllFllPllTracking::set_channel_queue(
+        concurrent_queue<int> *channel_internal_queue)
+{
+    channel_internal_queue_ = channel_internal_queue;
+
+    tracking_->set_channel_queue(channel_internal_queue_);
+
+}
+void GpsL1CaDllFllPllTracking::set_prn_code_phase(signed int phase_samples)
+{
+    return tracking_->set_acq_code_phase((float)phase_samples);
+}
+
+void GpsL1CaDllFllPllTracking::set_doppler_freq_shift(float doppler_freq_hz)
+{
+    return tracking_->set_acq_doppler(doppler_freq_hz);
+}
+
+void GpsL1CaDllFllPllTracking::set_acq_sample_stamp(
+        unsigned long int sample_stamp)
+{
+    return tracking_->set_acq_sample_stamp(sample_stamp);
+}
+void GpsL1CaDllFllPllTracking::connect(gr_top_block_sptr top_block)
+{
+    //nothing to connect, now the tracking uses gr_sync_decimator
+}
+
+void GpsL1CaDllFllPllTracking::disconnect(gr_top_block_sptr top_block)
+{
+    //nothing to disconnect, now the tracking uses gr_sync_decimator
+}
+
+gr_basic_block_sptr GpsL1CaDllFllPllTracking::get_left_block()
+{
+    return tracking_;
+}
+
+gr_basic_block_sptr GpsL1CaDllFllPllTracking::get_right_block()
+{
+    return tracking_;
+}
+

src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h

@@ -0,0 +1,104 @@
+/*!
+ * \file gps_l1_ca_dll_fll_pll_tracking.h
+ * \brief code DLL + carrier FLL/PLL tracking
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
+ * according to the algorithms described in [1]
+ * [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
+ * Applications, Second Edition, Artech House Publishers, 2005.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_
+#define GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_
+
+#include "tracking_interface.h"
+
+#include "gps_l1_ca_dll_fll_pll_tracking_cc.h"
+
+#include <gnuradio/gr_msg_queue.h>
+
+class ConfigurationInterface;
+
+
+class GpsL1CaDllFllPllTracking : public TrackingInterface
+{
+
+public:
+
+  GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration,
+            std::string role,
+            unsigned int in_streams,
+            unsigned int out_streams,
+            gr_msg_queue_sptr queue);
+
+    virtual ~GpsL1CaDllFllPllTracking();
+
+    std::string role()
+    {
+        return role_;
+    }
+    std::string implementation()
+    {
+        return "tracking";
+    }
+    size_t item_size()
+    {
+        return item_size_;
+    }
+
+    void connect(gr_top_block_sptr top_block);
+    void disconnect(gr_top_block_sptr top_block);
+    gr_basic_block_sptr get_left_block();
+    gr_basic_block_sptr get_right_block();
+
+    void set_satellite(unsigned int satellite);
+    void set_channel(unsigned int channel);
+    void set_prn_code_phase(signed int phase_samples);
+    void set_doppler_freq_shift(float doppler_freq_hz);
+    void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
+
+    void start_tracking();
+
+    void set_acq_sample_stamp(unsigned long int sample_stamp);
+
+private:
+
+    gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_;
+    size_t item_size_;
+
+    unsigned int satellite_;
+    unsigned int channel_;
+
+    std::string role_;
+    unsigned int in_streams_;
+    unsigned int out_streams_;
+    gr_msg_queue_sptr queue_;
+    concurrent_queue<int> *channel_internal_queue_;
+};
+
+#endif // GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_

src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc

@@ -52,31 +52,45 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
             queue)
 {
 
-    std::string default_item_type = "gr_complex";
-    std::string default_dump_filename = "./tracking.dat";
-
     DLOG(INFO) << "role " << role;
-    DLOG(INFO) << "vector length " << vector_length_;
+    //DLOG(INFO) << "vector length " << vector_length;
 
-    item_type_ = configuration->property(role + ".item_type",
+    //################# CONFIGURATION PARAMETERS ########################
-            default_item_type);
+
-    vector_length_ = configuration->property(role + ".vector_length", 2048);
+    int fs_in;
-    fs_in_ = configuration->property(role + ".fs_in", 2048000);
+    int vector_length;
-    if_ = configuration->property(role + ".if", 0);
+    int f_if;
-    dump_ = configuration->property(role + ".dump", false);
+    bool dump;
-    dump_filename_ = configuration->property(role + ".dump_filename",
+    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(role + ".fs_in", 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 = "./tracking.dat";
+    dump_filename = configuration->property(role + ".dump_filename",
             default_dump_filename); //unused!
 
-
+    //################# MAKE TRACKING GNURadio object ###################
-    if (item_type_.compare("gr_complex") == 0)
+    if (item_type.compare("gr_complex") == 0)
     {
         item_size_ = sizeof(gr_complex);
-        tracking_ = gps_l1_ca_dll_pll_make_tracking_cc(satellite_, if_,
+        tracking_ = gps_l1_ca_dll_pll_make_tracking_cc(satellite_, f_if,
-                fs_in_, vector_length_, queue_, dump_);
+                fs_in, vector_length, queue_, dump,pll_bw_hz,dll_bw_hz,early_late_space_chips);
     }
     else
     {
-        LOG_AT_LEVEL(WARNING) << item_type_ << " unknown tracking item type.";
+        LOG_AT_LEVEL(WARNING) << item_type << " unknown tracking item type.";
     }
 
     DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
@@ -91,6 +105,9 @@ void GpsL1CaDllPllTracking::start_tracking()
     tracking_->start_tracking();
 }
 
+/*!
+ * Set satellite ID
+ */
 void GpsL1CaDllPllTracking::set_satellite(unsigned int satellite)
 {
     satellite_ = satellite;
@@ -98,12 +115,18 @@ void GpsL1CaDllPllTracking::set_satellite(unsigned int satellite)
     DLOG(INFO) << "satellite set to " << satellite_;
 }
 
+/*!
+ * Set tracking channel unique ID
+ */
 void GpsL1CaDllPllTracking::set_channel(unsigned int channel)
 {
     channel_ = channel;
     tracking_->set_channel(channel);
 }
 
+/*!
+ * Set tracking channel internal queue
+ */
 void GpsL1CaDllPllTracking::set_channel_queue(
         concurrent_queue<int> *channel_internal_queue)
 {
@@ -112,16 +135,23 @@ void GpsL1CaDllPllTracking::set_channel_queue(
     tracking_->set_channel_queue(channel_internal_queue_);
 
 }
+/*!
+ * Set acquisition code phase in samples
+ */
 void GpsL1CaDllPllTracking::set_prn_code_phase(signed int phase_samples)
 {
     return tracking_->set_acq_code_phase((float)phase_samples);
 }
-
+/*!
+ * Set acquisition Doppler frequency in Hz.
+ */
 void GpsL1CaDllPllTracking::set_doppler_freq_shift(float doppler_freq_hz)
 {
     return tracking_->set_acq_doppler(doppler_freq_hz);
 }
-
+/*!
+ * Set acquisition sample stamp in samples, in order to detect the delay between acquisition and tracking
+ */
 void GpsL1CaDllPllTracking::set_acq_sample_stamp(
         unsigned long int sample_stamp)
 {

src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h

@@ -90,14 +90,9 @@ private:
 
     gps_l1_ca_dll_pll_tracking_cc_sptr tracking_;
     size_t item_size_;
-    std::string item_type_;
+
-    unsigned int vector_length_;
     unsigned int satellite_;
     unsigned int channel_;
-    long fs_in_;
-    long if_;
-    bool dump_;
-    std::string dump_filename_;
 
     std::string role_;
     unsigned int in_streams_;

src/algorithms/tracking/adapters/jamfile.jam

@@ -1,3 +1,4 @@
 project : build-dir ../../../../build ;
 
 obj gps_l1_ca_dll_pll_tracking : gps_l1_ca_dll_pll_tracking.cc ;
+obj gps_l1_ca_dll_fll_pll_tracking : gps_l1_ca_dll_fll_pll_tracking.cc ;

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc

@@ -0,0 +1,510 @@
+/*!
+ * \file gps_l1_ca_dll_fll_pll_tracking_cc.cc
+ * \brief code DLL + carrier FLL/PLL tracking
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
+ * according to the algorithms described in [1]
+ * [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
+ * Applications, Second Edition, Artech House Publishers, 2005.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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_l1_ca_dll_fll_pll_tracking_cc.h"
+#include "gps_sdr_signal_processing.h"
+#include "GPS_L1_CA.h"
+#include "tracking_discriminators.h"
+#include "CN_estimators.h"
+#include "tracking_FLL_PLL_filter.h"
+
+#include "control_message_factory.h"
+#include <boost/lexical_cast.hpp>
+#include <iostream>
+#include <sstream>
+#include <cmath>
+#include "math.h"
+
+#include <gnuradio/gr_io_signature.h>
+
+#include <glog/log_severity.h>
+#include <glog/logging.h>
+
+/*!
+ * \todo Include in definition header file
+ */
+#define CN0_ESTIMATION_SAMPLES 10
+
+using google::LogMessage;
+
+gps_l1_ca_dll_fll_pll_tracking_cc_sptr
+gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
+			int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
+
+	return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new gps_l1_ca_dll_fll_pll_tracking_cc(satellite, if_freq,
+			fs_in, vector_length, queue, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips));
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::forecast (int noutput_items,
+    gr_vector_int &ninput_items_required){
+    ninput_items_required[0] =d_vector_length*2; //set the required available samples in each call
+}
+
+gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
+		int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
+	    gr_block ("gps_l1_ca_dll_fll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
+	              gr_make_io_signature(5, 5, sizeof(float))) {
+		//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
+		//		gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
+	// initialize internal vars
+	d_queue = queue;
+	d_dump = dump;
+	d_satellite = satellite;
+	d_if_freq = if_freq;
+	d_fs_in = fs_in;
+	d_vector_length = vector_length;
+	d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
+
+	// Initialize tracking variables ==========================================
+	d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order);
+
+    // Get space for a vector with the C/A code replica sampled 1x/chip
+    d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2];
+    // Get space for the resampled early / prompt / late local replicas
+    d_early_code= new gr_complex[d_vector_length*2];
+    d_prompt_code=new gr_complex[d_vector_length*2];
+    d_late_code=new gr_complex[d_vector_length*2];
+    // space for carrier wipeoff LO vector
+    d_carr_sign=new gr_complex[d_vector_length*2];
+
+    // sample synchronization
+    d_sample_counter=0;
+    d_acq_sample_stamp=0;
+    d_last_seg=0;// this is for debug output only
+
+    d_enable_tracking=false;
+
+    d_current_prn_length_samples=(int)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=5;
+
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
+
+	/*!
+	 *  correct the code phase according to the delay between acq and trk
+	 */
+	unsigned long int acq_trk_diff_samples;
+	float acq_trk_diff_seconds;
+	acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
+	acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
+	//doppler effect
+	// Fd=(C/(C+Vr))*F
+	float radial_velocity;
+	radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
+	// new chip and prn sequence periods based on acq Doppler
+	float T_chip_mod_seconds;
+	float T_prn_mod_seconds;
+	float T_prn_mod_samples;
+	d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
+	T_chip_mod_seconds=1/d_code_freq_hz;
+	T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
+	T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
+    d_next_prn_length_samples=round(T_prn_mod_samples);
+	//compute the code phase chips prediction
+	float delta_T_prn_samples;
+	float delay_correction_samples;
+	delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
+	delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
+	d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
+	if (d_acq_code_phase_samples<0){
+		d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
+	}
+	d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
+	// DLL/PLL filter initialization
+	d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
+	d_FLL_wait=1;
+
+	// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
+    code_gen_conplex(&d_ca_code[1],d_satellite,0);
+    d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
+    d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
+
+	d_carrier_lock_fail_counter=0;
+	d_Prompt_prev=0;
+	d_rem_code_phase_samples=0;
+	d_rem_carr_phase=0;
+	d_FLL_discriminator_hz=0;
+	d_rem_code_phase_samples=0;
+	d_next_rem_code_phase_samples=0;
+	d_acc_carrier_phase_rad=0;
+
+	// ############# ENABLE DATA FILE LOG #################
+	if (d_dump==true)
+	{
+		if (d_dump_file.is_open()==false)
+		{
+			try {
+				d_dump_filename="track_ch"; //base path and name for the tracking log file
+				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);
+				std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
+			}
+			catch (std::ifstream::failure e) {
+				std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
+			}
+		}
+	}
+	// DEBUG OUTPUT
+	std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
+	DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
+
+	// enable tracking
+	d_pull_in=true;
+	d_enable_tracking=true;
+
+	std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [chips]= "<<d_acq_code_phase_samples<<"\r\n";
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code()
+{
+	float tcode_chips;
+	float rem_code_phase_chips;
+	float code_phase_step_chips;
+	int associated_chip_index;
+	int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
+	code_phase_step_chips=d_code_freq_hz/((float)d_fs_in);
+	rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
+	// unified loop for E, P, L code vectors
+	tcode_chips=-rem_code_phase_chips;
+    for (int i=0;i<d_current_prn_length_samples;i++)
+    {
+        associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips));
+        d_early_code[i] = d_ca_code[associated_chip_index];
+        associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
+        d_prompt_code[i] = d_ca_code[associated_chip_index];
+        associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
+        d_late_code[i] = d_ca_code[associated_chip_index];
+        tcode_chips=tcode_chips+code_phase_step_chips;
+    }
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
+{
+    float phase, phase_step;
+    phase_step = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
+    phase=d_rem_carr_phase;
+    for(int i = 0; i < d_current_prn_length_samples; i++) {
+        d_carr_sign[i] = gr_complex(cos(phase),sin(phase));
+        phase += phase_step;
+    }
+    d_rem_carr_phase=fmod(phase,TWO_PI);
+    d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase;
+}
+
+gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
+	d_dump_file.close();
+    delete d_ca_code;
+    delete d_early_code;
+    delete d_prompt_code;
+    delete d_late_code;
+    delete d_carr_sign;
+    delete d_Prompt_buffer;
+}
+
+/*! Tracking signal processing
+ * Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
+ */
+
+int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
+    gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
+
+	if (d_enable_tracking==true){
+		/*!
+		 * Receiver signal alignment
+		 */
+	    if (d_pull_in==true)
+	    {
+	        int samples_offset=round(d_acq_code_phase_samples);
+	        d_sample_counter+=samples_offset; //count for the processed samples
+	        d_pull_in=false;
+	        std::cout<<" samples_offset "<<samples_offset<<"\r\n";
+	        consume_each(samples_offset); //shift input to perform alignement with local replica
+	        return 1;
+	    }
+	    // get the sample in and out pointers
+		const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
+		float **out = (float **) &output_items[0]; //block output streams pointer
+
+		// Update the prn length based on code freq (variable) and
+		// sampling frequency (fixed)
+		// variable code PRN sample block size
+	    d_current_prn_length_samples=d_next_prn_length_samples;
+
+		update_local_code();
+		update_local_carrier();
+
+		gr_complex bb_signal_sample(0,0);
+
+		d_Prompt_prev=d_Prompt; // for the FLL discriminator
+		d_Early=gr_complex(0,0);
+		d_Prompt=gr_complex(0,0);
+		d_Late=gr_complex(0,0);
+
+		// perform Early, Prompt and Late correlation
+		/*!
+		 * \todo Use SIMD-enabled correlators
+		 */
+		for(int i=0;i<d_current_prn_length_samples;i++) {
+			//Perform the carrier wipe-off
+			bb_signal_sample = in[i] * d_carr_sign[i];
+			// Now get early, late, and prompt values for each
+			d_Early += bb_signal_sample*d_early_code[i];
+			d_Prompt += bb_signal_sample*d_prompt_code[i];
+			d_Late += bb_signal_sample*d_late_code[i];
+		}
+
+		/*!
+		 * DLL, FLL, and PLL discriminators
+		 */
+		// Compute DLL error
+		float code_error_chips;
+		code_error_chips=dll_nc_e_minus_l_normalized(d_Early,d_Late);
+
+		//compute FLL error
+		float correlation_time_s;
+		correlation_time_s=((float)d_current_prn_length_samples)/(float)d_fs_in;
+		if (d_FLL_wait==1)
+		{
+		   d_Prompt_prev=d_Prompt;
+		   d_FLL_wait=0;
+		}else{
+		   d_FLL_discriminator_hz=fll_four_quadrant_atan(d_Prompt_prev, d_Prompt, 0, correlation_time_s)/(float)TWO_PI;
+		   d_Prompt_prev=d_Prompt;
+		   d_FLL_wait=1;
+		}
+
+		// Compute PLL error
+		float PLL_discriminator_hz;
+		 PLL_discriminator_hz=pll_cloop_two_quadrant_atan(d_Prompt)/(float)TWO_PI;
+		 //PLL_discriminator_hz=pll_four_quadrant_atan(d_Prompt)/(float)TWO_PI;
+
+		 /*!
+		  * \todo Update FLL assistance algorithm!
+		  */
+		 if (((float)d_sample_counter-(float)d_acq_sample_stamp)/(float)d_fs_in>3)
+		 {
+			 d_FLL_discriminator_hz=0; //disconnect the FLL after the initial lock
+		 }
+		/*!
+		 * DLL and FLL+PLL filter and get current carrier Doppler and code frequency
+		 */
+		 float carr_nco_hz;
+		 carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s);
+		 d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
+		 d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips;
+
+		// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
+		if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
+		{
+			// fill buffer with prompt correlator output values
+			d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
+			d_cn0_estimation_counter++;
+		}else{
+			d_cn0_estimation_counter=0;
+			d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
+			d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
+			// ###### TRACKING UNLOCK NOTIFICATION #####
+			int tracking_message;
+	        if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
+	        {
+	             d_carrier_lock_fail_counter++;
+	        }else{
+	        	if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
+	        }
+	        if (d_carrier_lock_fail_counter>300)
+	        {
+	        	std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
+	        	tracking_message=3; //loss of lock
+	        	d_channel_internal_queue->push(tracking_message);
+	        	d_carrier_lock_fail_counter=0;
+	        	d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
+
+	        }
+	        //std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
+		}
+
+		// ########### Output the tracking data to navigation and PVT ##########
+		// Output channel 1: Prompt correlator output Q
+		*out[0]=d_Early.real();
+		// Output channel 2: Prompt correlator output I
+		*out[1]=d_Early.imag();
+		// Output channel 3: PRN absolute delay [ms]
+		*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
+		// Output channel 4: PRN code error [ms]
+		*out[3]=d_acc_carrier_phase_rad;
+
+		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;
+			prompt_I=d_Prompt.imag();
+			prompt_Q=d_Prompt.real();
+			tmp_E=std::abs<float>(d_Early);
+			tmp_P=std::abs<float>(d_Prompt);
+			tmp_L=std::abs<float>(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*)&tmp_float, sizeof(float));
+				// accumulated carrier phase
+				d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
+
+				// carrier and code frequency
+				d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
+				d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
+
+				//PLL commands
+				d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
+				d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
+
+				//DLL commands
+				d_dump_file.write((char*)&code_error_chips, sizeof(float));
+				d_dump_file.write((char*)&code_error_chips, sizeof(float));
+
+				// CN0 and carrier lock test
+				d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
+				d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
+
+				// AUX vars (for debug purposes)
+				tmp_float=d_FLL_discriminator_hz;
+				d_dump_file.write((char*)&tmp_float, sizeof(float));
+				tmp_float=0.0;
+				d_dump_file.write((char*)&tmp_float, sizeof(float));
+	      	 }
+			  catch (std::ifstream::failure e) {
+				std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
+			  }
+		}
+
+		// ########## 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<<"t="<<d_last_seg<<std::endl;
+				std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
+				std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
+			}
+		}else
+		{
+			if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
+			{
+				d_last_seg=floor(d_sample_counter/d_fs_in);
+				std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
+				std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
+			}
+		}
+
+		//predict the next loop PRN period length prediction
+		//float T_chip_seconds,T_prn_seconds,T_prn_samples;
+		//T_chip_seconds=1/d_code_freq_hz;
+		//T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS-d_rem_code_phase_chips*T_chip_seconds;
+		//T_prn_samples=T_prn_seconds*(float)d_fs_in;
+		//d_next_prn_length_samples=round(T_prn_samples);
+
+		float T_chip_seconds;
+		float T_prn_seconds;
+		float T_prn_samples;
+		float K_blk_samples;
+		T_chip_seconds=1/d_code_freq_hz;
+		T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
+		T_prn_samples=T_prn_seconds*d_fs_in;
+
+		d_rem_code_phase_samples=d_next_rem_code_phase_samples;
+		K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
+	    d_next_prn_length_samples=round(K_blk_samples);
+	    d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
+
+
+	}
+	consume_each(d_current_prn_length_samples); // this is necesary 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_l1_ca_dll_fll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
+	d_acq_code_phase_samples = code_phase;
+	LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_doppler(float doppler) {
+	d_acq_carrier_doppler_hz = doppler;
+	LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::set_satellite(unsigned int satellite) {
+	d_satellite = satellite;
+	LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel(unsigned int channel) {
+	d_channel = channel;
+	LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
+{
+    d_acq_sample_stamp = sample_stamp;
+}
+
+void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
+{
+    d_channel_internal_queue = channel_internal_queue;
+}

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h

@@ -0,0 +1,201 @@
+/*!
+ * \file gps_l1_ca_dll_fll_pll_tracking_cc.h
+ * \brief code DLL + carrier FLL/PLL tracking
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * This file implements the code Delay Locked Loop (DLL) +
+ * carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
+ * according to the algorithms described in [1]
+ * [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
+ * Applications, Second Edition, Artech House Publishers, 2005.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H
+#define	GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H
+
+#include <fstream>
+
+#include <gnuradio/gr_block.h>
+#include <gnuradio/gr_msg_queue.h>
+//#include <gnuradio/gr_sync_decimator.h>
+
+#include "gps_sdr_signal_processing.h"
+#include "tracking_FLL_PLL_filter.h"
+
+#include <queue>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/thread.hpp>
+#include "concurrent_queue.h"
+
+class gps_l1_ca_dll_fll_pll_tracking_cc;
+typedef boost::shared_ptr<gps_l1_ca_dll_fll_pll_tracking_cc>
+        gps_l1_ca_dll_fll_pll_tracking_cc_sptr;
+
+gps_l1_ca_dll_fll_pll_tracking_cc_sptr
+gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite,
+										long if_freq,
+										long fs_in,
+										unsigned int vector_length,
+										gr_msg_queue_sptr queue,
+										bool dump,
+										int order,
+	                                    float fll_bw_hz,
+	                                    float pll_bw_hz,
+	                                    float dll_bw_hz,
+	                                    float early_late_space_chips);
+
+//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
+class gps_l1_ca_dll_fll_pll_tracking_cc: public gr_block
+{
+
+private:
+
+    friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr
+    gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite,
+										   long if_freq,
+										   long fs_in, unsigned
+										   int vector_length,
+										   gr_msg_queue_sptr queue,
+										   bool dump,
+										   int order,
+										   float fll_bw_hz,
+										   float pll_bw_hz,
+										   float dll_bw_hz,
+										   float early_late_space_chips);
+
+    gps_l1_ca_dll_fll_pll_tracking_cc(unsigned int satellite,
+									  long if_freq,
+									  long fs_in, unsigned
+									  int vector_length,
+									  gr_msg_queue_sptr queue,
+									  bool dump,
+									  int order,
+									  float fll_bw_hz,
+									  float pll_bw_hz,
+									  float dll_bw_hz,
+									  float early_late_space_chips);
+
+    void CN0_estimation_and_lock_detectors();
+
+    // class private vars
+    gr_msg_queue_sptr d_queue;
+    concurrent_queue<int> *d_channel_internal_queue;
+    unsigned int d_vector_length;
+    bool d_dump;
+    unsigned int d_satellite;
+    unsigned int d_channel;
+    int d_last_seg;
+    long d_if_freq;
+    long d_fs_in;
+
+    gr_complex* d_ca_code;
+
+    gr_complex* d_early_code;
+    gr_complex* d_late_code;
+    gr_complex* d_prompt_code;
+
+    gr_complex* d_carr_sign;
+
+	gr_complex d_Early;
+	gr_complex d_Prompt;
+	gr_complex d_Prompt_prev;
+	gr_complex d_Late;
+
+	float d_early_late_spc_chips;
+
+
+	float d_carrier_doppler_hz;
+	float d_code_freq_hz;
+	int d_current_prn_length_samples;
+	int d_next_prn_length_samples;
+	int d_FLL_wait;
+    float d_rem_carr_phase;
+    float d_rem_code_phase_samples;
+    float d_next_rem_code_phase_samples;
+    bool d_pull_in;
+
+    // acquisition
+    float d_acq_code_phase_samples;
+    float d_acq_carrier_doppler_hz;
+
+    // FLL + PLL filter
+    float d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory
+    tracking_FLL_PLL_filter d_carrier_loop_filter;
+    float d_acc_carrier_phase_rad;
+
+    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;
+    float d_carrier_lock_test;
+    float d_CN0_SNV_dB_Hz;
+
+    float d_carrier_lock_threshold;
+
+    int d_carrier_lock_fail_counter;
+
+    bool d_enable_tracking;
+
+    std::string d_dump_filename;
+    std::ofstream d_dump_file;
+
+public:
+
+    ~gps_l1_ca_dll_fll_pll_tracking_cc();
+
+    void set_satellite(unsigned int satellite);
+    void set_channel(unsigned int channel);
+    void set_acq_code_phase(float code_phase);
+    void set_acq_doppler(float doppler);
+    void start_tracking();
+    void update_local_code();
+    void update_local_carrier();
+    void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz);
+    void set_acq_sample_stamp(unsigned long int sample_stamp);
+    void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
+
+    /*!
+     * \brief just like gr_block::general_work, only this arranges to call consume_each for you
+     *
+     * The user must override work to define the signal processing code
+     */
+    //virtual int work (int noutput_items,
+    //                  gr_vector_const_void_star &input_items,
+    //                gr_vector_void_star &output_items) = 0;
+
+   //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
+
+    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);
+
+};
+
+#endif //GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -32,15 +32,12 @@
  *
  * -------------------------------------------------------------------------
  */
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
 
 #include "gps_l1_ca_dll_pll_tracking_cc.h"
 #include "gps_sdr_signal_processing.h"
-
+#include "tracking_discriminators.h"
-#include "gps_sdr_simd.h"
+#include "CN_estimators.h"
-#include "gps_sdr_x86.h"
+#include "GPS_L1_CA.h"
 
 #include "control_message_factory.h"
 #include <boost/lexical_cast.hpp>
@@ -51,8 +48,6 @@
 
 #include <gnuradio/gr_io_signature.h>
 
-
-
 #include <glog/log_severity.h>
 #include <glog/logging.h>
 
@@ -65,19 +60,19 @@ using google::LogMessage;
 
 gps_l1_ca_dll_pll_tracking_cc_sptr
 gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
-			int vector_length, gr_msg_queue_sptr queue, bool dump) {
+			int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
 
 	return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
-			fs_in, vector_length, queue, dump));
+			fs_in, vector_length, queue, dump, pll_bw_hz, dll_bw_hz, early_late_space_chips));
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
     gr_vector_int &ninput_items_required){
-    ninput_items_required[0] =d_vector_length*2; //set the required available samples in each call
+    ninput_items_required[0] =(int)d_vector_length*2; //set the required available samples in each call
 }
 
 gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
-		int vector_length, gr_msg_queue_sptr queue, bool dump) :
+		int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
 	    gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
 	              gr_make_io_signature(5, 5, sizeof(float))) {
 
@@ -91,31 +86,19 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
 	d_fs_in = fs_in;
 	d_vector_length = vector_length;
 
-	// Initialize tracking variables ==========================================
+	//std::cout<<"pll_bw_hz= "<<pll_bw_hz<<"dll_bw_hz="<<dll_bw_hz<<"\r\n";
-	/*!
+
-	 * \todo Include PLL and DLL filter setting in configuration file
+	// 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 = 0.5; // Define early-late offset (in chips)
+	d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
-	d_pdi_code = 0.001;// Summation interval for code
-	d_dllnoisebandwidth=1; //Hz
-	d_dlldampingratio=0.7;
-	calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
-
-	//--- PLL variables --------------------------------------------------------
-	d_pdi_carr = 0.001;// Summation interval for carrier
-	d_plldampingratio=0.7;
-	d_pllnoisebandwidth=50;
-
-	//Calculate filter coefficient values
-	calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
 
 	// Initialization of local code replica
-
-	d_code_length=1023;
     // Get space for a vector with the C/A code replica sampled 1x/chip
-    d_ca_code=new gr_complex[d_code_length+2];
+    d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2];
 
     // Get space for the resampled early / prompt / late local replicas
     d_early_code= new gr_complex[d_vector_length*2];
@@ -124,29 +107,14 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
 
     // space for carrier wipeoff and signal baseband vectors
     d_carr_sign=new gr_complex[d_vector_length*2];
-    d_bb_sign=new gr_complex[d_vector_length*2];
 
     //--- Perform initializations ------------------------------
-
     // define initial code frequency basis of NCO
-    d_code_freq      = 1023000; //Hz
+    d_code_freq_hz      = GPS_L1_CA_CODE_RATE_HZ;
     // define residual code phase (in chips)
-    d_rem_code_phase  = 0.0;
+    d_rem_code_phase_samples  = 0.0;
-    // define carrier frequency which is used over whole tracking period
-    // it must be set with set_acq_code_phase() and set_acq_doppler()
-
     // define residual carrier phase
-    d_rem_carr_phase  = 0.0;
+    d_rem_carr_phase_rad  = 0.0;
-
-    // code tracking loop parameters
-    d_old_code_nco   = 0.0;
-    d_old_code_error = 0.0;
-
-    // carrier/Costas loop parameters
-    d_old_carr_nco   = 0.0;
-    d_old_carr_error = 0.0;
-
-    d_absolute_code_phase_samples = 0;
 
     // sample synchronization
     d_sample_counter=0;
@@ -156,74 +124,68 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
     d_pull_in=false;
     d_last_seg=0;
 
-    d_blksize=d_vector_length;
+    d_current_prn_length_samples=(int)d_vector_length;
-    d_loops_count=0;
 
     // CN0 estimation and lock detector buffers
     d_cn0_estimation_counter=0;
-    d_P_I_buffer=new float[CN0_ESTIMATION_SAMPLES];
+    d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES];
-    d_P_Q_buffer=new float[CN0_ESTIMATION_SAMPLES];
     d_carrier_lock_test=1;
-    d_SNR_SNV=0;
+    d_CN0_SNV_dB_Hz=0;
-    d_SNR_SNV_dB_Hz=0;
-    d_SNR_MM=0;
     d_carrier_lock_fail_counter=0;
     d_carrier_lock_threshold=5;
-
 }
 
-void gps_l1_ca_dll_pll_tracking_cc::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
-	// Solve natural frequency
-	float Wn;
-	Wn = lbw*8*zeta / (4*zeta*zeta + 1);
-	// solve for t1 & t2
-	*tau1 = k / (Wn * Wn);
-	*tau2 = (2.0 * zeta) / Wn;
-}
-
-
 void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
+	/*!
+	 *  correct the code phase according to the delay between acq and trk
+	 */
+	unsigned long int acq_trk_diff_samples;
+	float acq_trk_diff_seconds;
+	acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
+	acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
+	//doppler effect
+	// Fd=(C/(C+Vr))*F
+	float radial_velocity;
+	radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
+	// new chip and prn sequence periods based on acq Doppler
+	float T_chip_mod_seconds;
+	float T_prn_mod_seconds;
+	float T_prn_mod_samples;
+	d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
+	T_chip_mod_seconds=1/d_code_freq_hz;
+	T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
+	T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
 
-    unsigned long int acq_sample_difference;
+    d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
-    int trk_corrected_code_phase;
+    d_next_prn_length_samples=round(T_prn_mod_samples);
-
-    acq_sample_difference=this->d_sample_counter-d_acq_sample_stamp-d_vector_length;
-
-    float velocity_ratio,code_freq_mod,T_prn,T_prn_mod,T_chip_mod;
-
-    const float carrier_freq=1575420000;
-    velocity_ratio=(carrier_freq+d_carrier_doppler)/carrier_freq;
-
-    code_freq_mod=velocity_ratio*d_code_freq;
-
-    T_prn=(1/d_code_freq)*(float)d_code_length;
-
-    T_chip_mod=1/code_freq_mod;
-    T_prn_mod=T_chip_mod*(float)d_code_length;
-
 	//compute the code phase chips prediction
-    trk_corrected_code_phase=round(fmod((d_code_phase+(float)acq_sample_difference+(T_prn-T_prn_mod)*((float)acq_sample_difference/(float)d_vector_length)*(float)d_fs_in),(float)d_vector_length));
+	float delta_T_prn_samples;
-
+	float delay_correction_samples;
-    if (trk_corrected_code_phase<0)
+	delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
-    {
+	delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
-        trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
+	d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
+	if (d_acq_code_phase_samples<0){
+		d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
 	}
-    d_absolute_code_phase_samples=(float)trk_corrected_code_phase;
+	d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
+	// DLL/PLL filter initialization
+	d_carrier_loop_filter.initialize(d_carrier_doppler_hz); //initialize the carrier filter
+	d_code_loop_filter.initialize(d_acq_code_phase_samples); //initialize the code filter
 
-    // generate local reference ALWAYS starting at chip 1, not corrected
+	// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
     code_gen_conplex(&d_ca_code[1],d_satellite,0);
+    d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
+    d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
 
-    // Then make it possible to do early and late versions
+	d_carrier_lock_fail_counter=0;
-    d_ca_code[0]=d_ca_code[1023];
+	d_rem_code_phase_samples=0;
-    d_ca_code[1024]=d_ca_code[1];
+	d_next_rem_code_phase_samples=0;
-
+	d_rem_carr_phase_rad=0;
-    DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
+	d_acc_carrier_phase_rad=0;
 
+	// ############# ENABLE DATA FILE LOG #################
 	if (d_dump==true)
 	{
-        //std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
-        //d_dump_filename_str<<"./data/trk_epl_CH_"<<this->d_channel<<"_SAT_"<<this->d_satellite<<".dat";
-        //d_dump_filename=d_dump_filename_str.str();
 		if (d_dump_file.is_open()==false)
 		{
 			try {
@@ -239,59 +201,58 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
 			}
 		}
 	}
-    d_carrier_lock_fail_counter=0;
+	// DEBUG OUTPUT
+	std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
+	DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
+	// enable tracking
 	d_pull_in=true;
 	d_enable_tracking=true;
-    std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID "<< this->d_satellite+1 << std::endl;
+	std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
 }
 
-void gps_l1_ca_dll_pll_tracking_cc::update_local_code_refs()
+void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
 {
-	float tcode;
+	float tcode_chips;
+	float rem_code_phase_chips;
 	int associated_chip_index;
+	int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
 	// unified loop for E, P, L code vectors
-    for (unsigned int i=0;i<d_blksize;i++)
+	rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
+	tcode_chips=-rem_code_phase_chips;
+    for (int i=0;i<d_current_prn_length_samples;i++)
     {
-        tcode=i*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
+        associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips));
-        associated_chip_index=ceil(fmod(tcode,d_code_length));
         d_early_code[i] = d_ca_code[associated_chip_index];
-        tcode += d_early_late_spc;
+        associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
-        associated_chip_index = ceil(fmod(tcode, d_code_length));
         d_prompt_code[i] = d_ca_code[associated_chip_index];
-        tcode += d_early_late_spc;
+        associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
-        associated_chip_index = ceil(fmod(tcode, d_code_length));
         d_late_code[i] = d_ca_code[associated_chip_index];
+        tcode_chips=tcode_chips+d_code_phase_step_chips;
     }
-
-    //**** Option 1: Keep the number of samples per PRN period constant and equal to the nominal value
-    //****           and record the size mismatch in a var: d_rem_code_phase
-    //max_tcode=((float)d_vector_length-1.0)*d_code_phase_step+d_rem_code_phase;
-    //d_rem_code_phase = (max_tcode + d_code_phase_step) - 1023.0;
-    //d_rem_code_phase = d_rem_code_phase+((float)d_vector_length-1023.0*(1.0/d_code_freq)*(float)d_fs_in)*d_code_phase_step;
-
-    //**** Option 2: Each loop, compute the new PRN sequence code length according to the estimated Doppler
-    tcode=d_blksize*d_code_phase_step+d_rem_code_phase;
-    d_rem_code_phase = tcode - 1023.0; //prompt remaining code phase
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
 {
-        float phase, phase_step;
+        float phase_rad, phase_step_rad;
 
-        phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
+        phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
-        phase=d_rem_carr_phase;
+        phase_rad=d_rem_carr_phase_rad;
-        for(unsigned int i = 0; i < d_blksize; i++) {
+        for(int i = 0; i < d_current_prn_length_samples; i++) {
-            d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
+            d_carr_sign[i] = gr_complex(cos(phase_rad),sin(phase_rad));
-            phase += phase_step;
+            phase_rad += phase_step_rad;
         }
-        d_rem_carr_phase=fmod(phase,TWO_PI);
+        d_rem_carr_phase_rad=fmod(phase_rad,TWO_PI);
+        d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase_rad;
 }
 
 gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
-    /*!
-     * \todo free memory!!
-     */
 	d_dump_file.close();
+    delete d_ca_code;
+    delete d_early_code;
+    delete d_prompt_code;
+    delete d_late_code;
+    delete d_carr_sign;
+    delete d_Prompt_buffer;
 }
 
 /*! Tracking signal processing
@@ -300,162 +261,91 @@ gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
 
 int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
     gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
-    d_loops_count++;
 	if (d_enable_tracking==true){
-
 	    if (d_pull_in==true)
 	    {
-	        int samples_offset=ceil(d_absolute_code_phase_samples);
+	        int samples_offset=ceil(d_acq_code_phase_samples);
-	        d_code_phase_ms=(d_absolute_code_phase_samples*1000.0)/(float)d_fs_in;
+	        consume_each(d_acq_code_phase_samples); //shift input to perform alignement with local replica
-	        consume_each(samples_offset); //shift input to perform alignement with local replica
 	        d_sample_counter+=samples_offset; //count for the processed samples
 	        d_pull_in=false;
 	        return 1;
 	    }
+
+	    d_current_prn_length_samples=d_next_prn_length_samples;
+
 		float carr_error;
 		float carr_nco;
 		float code_error;
 		float code_nco;
-		float tmp_E,tmp_P,tmp_L;
 
 		const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
-
 		float **out = (float **) &output_items[0];
 
+		update_local_code();
+		update_local_carrier();
+
+		gr_complex bb_signal_sample(0,0);
+		d_Early=gr_complex(0,0);
+		d_Prompt=gr_complex(0,0);
+		d_Late=gr_complex(0,0);
+
+		// perform Early, Prompt and Late correlation
+		/*!
+		 * \todo Use SIMD-enabled correlators
+		 */
+		for(int i=0;i<d_current_prn_length_samples;i++) {
+			//Perform the carrier wipe-off
+			bb_signal_sample = in[i] * d_carr_sign[i];
+			// Now get early, late, and prompt values for each
+			d_Early += bb_signal_sample*d_early_code[i];
+			d_Prompt += bb_signal_sample*d_prompt_code[i];
+			d_Late += bb_signal_sample*d_late_code[i];
+		}
+		// Compute PLL error and update carrier NCO -
+		carr_error=pll_cloop_two_quadrant_atan(d_Prompt)/ (float)TWO_PI;
+		// Implement carrier loop filter and generate NCO command
+		carr_nco=d_carrier_loop_filter.get_carrier_nco(carr_error);
+		// Modify carrier freq based on NCO command
+		d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_nco;
+
+		// Compute DLL error and update code NCO
+		code_error=dll_nc_e_minus_l_normalized(d_Early,d_Late);
+		// Implement code loop filter and generate NCO command
+		code_nco=d_code_loop_filter.get_code_nco(code_error);
+		// Modify code freq based on NCO command
+		d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco;
+
 		// Update the phasestep based on code freq (variable) and
 		// sampling frequency (fixed)
-		// code_phase_step_per_sample = T_sample/T_chip
+		d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
-		d_code_phase_step = d_code_freq / (float)d_fs_in; //[chips]
 		// variable code PRN sample block size
-	    d_blksize=ceil((1023.0-d_rem_code_phase) / d_code_phase_step); //[samples]
+		float T_chip_seconds;
-
+		float T_prn_seconds;
-
+		float T_prn_samples;
-		//float rem_code_phase_samples=d_rem_code_phase/d_code_phase_step;
+		float K_blk_samples;
-		//d_absolute_code_phase_samples-=floor(rem_code_phase_samples);
+		T_chip_seconds=1/d_code_freq_hz;
-
+		T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
-		//d_rem_code_phase=d_code_phase_step*(rem_code_phase_samples-floor(rem_code_phase_samples));
+		T_prn_samples=T_prn_seconds*d_fs_in;
-
+		d_rem_code_phase_samples=d_next_rem_code_phase_samples;
-		this->update_local_code_refs();
+		K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
-		this->update_local_carrier();
+	    d_next_prn_length_samples=round(K_blk_samples);
-
+	    d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
-		d_E_I=0.0;
-		d_P_I=0.0;
-		d_L_I=0.0;
-
-		d_E_Q=0.0;
-		d_P_Q=0.0;
-		d_L_Q=0.0;
-
-		// perform Early, Prompt and Late correlation
-		for(unsigned int i=0;i<d_blksize;i++) {
-			//Perform the carrier wipe-off
-			d_bb_sign[i] = in[i] * d_carr_sign[i];
-			// Now get early, late, and prompt values for each
-			d_E_Q += d_early_code[i].real()*d_bb_sign[i].real();
-
-			d_E_I += d_early_code[i].real()*d_bb_sign[i].imag();
-
-			d_P_Q += d_prompt_code[i].real()*d_bb_sign[i].real();
-			d_P_I += d_prompt_code[i].real()*d_bb_sign[i].imag();
-
-			d_L_Q += d_late_code[i].real()*d_bb_sign[i].real();
-			d_L_I += d_late_code[i].real()*d_bb_sign[i].imag();
-		}
-
-		//float block_error_samples;
-		//block_error_samples=(float)d_vector_length-d_blksize;
-		//d_absolute_code_phase_samples=d_absolute_code_phase_samples+block_error_samples;
-		// Find PLL error and update carrier NCO -
-		// Implement carrier loop discriminator (phase detector)
-		//carr_error = atan(d_P.imag() / d_P.real()) / TWO_PI;
-
-		carr_error = atan(d_P_Q / d_P_I) / (float)TWO_PI;
-
-		// Implement carrier loop filter and generate NCO command
-
-		carr_nco = d_old_carr_nco+(d_tau2_carr/d_tau1_carr)*(carr_error - d_old_carr_error) + carr_error * (d_pdi_carr/d_tau1_carr);
-		d_old_carr_nco   = carr_nco;
-		d_old_carr_error = carr_error;
-
-		// Modify carrier freq based on NCO command
-		d_carrier_doppler = d_carr_freq_basis + carr_nco;
-
-		// Find DLL error and update code NCO -
-		//code_error = (abs(d_E)-abs(d_L))/(abs(d_E)+abs(d_L));
-
-		code_error = (sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q)-sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q))/(sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q)+sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q));
-
-		// Implement code loop filter and generate NCO command
-		code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(code_error - d_old_code_error) + code_error * (d_pdi_code/d_tau1_code);
-		d_old_code_nco   = code_nco;
-		d_old_code_error = code_error; //[chips]
-
-		// Modify code freq based on NCO command
-		d_code_freq = 1023000 - code_nco;
-
-
-		d_code_phase_ms+=(1023000/d_code_freq)-1.0;
 
 		/*!
-		 * \todo Code lock detector
+		 * \todo Improve the lock detection algorithm!
 		 */
-
 		// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
 		if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
 		{
 			// fill buffer with prompt correlator output values
-			d_P_I_buffer[d_cn0_estimation_counter]=d_P_I;
+			d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
-			d_P_Q_buffer[d_cn0_estimation_counter]=d_P_Q;
 			d_cn0_estimation_counter++;
 		}else{
-			// estimate CN0 and lock status using buffered values
-			   // MATLAB CODE
-			   //Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2;
-			   //Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2);
-			   //M2=Ptot;
-			   //M4=(1/N)*sum(abs(x((n-N+1):n)).^4);
-			   //SNR_SNV(count)=Psig/(Ptot-Psig);
-			   //SNR_MM(count)=sqrt(2*M2^2-M4)/(M2-Psig);
-			   // lock detector operation
-			   //NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2;
-			   //NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
-			   //LOCK(count)=NBD/NBP;
-			   //CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
-			float tmp_abs_I,tmp_abs_Q;
-			float tmp_sum_abs_I,tmp_sum_abs_Q;
-			float tmp_sum_sqr_I,tmp_sum_sqr_Q;
-			float Psig,Ptot,M2,M4;
-			float NBD,NBP;
-			Psig=0;
-			Ptot=0;
-			NBD=0;
-			NBP=0;
-			tmp_sum_abs_I=0;
-			tmp_sum_abs_Q=0;
-			tmp_sum_sqr_I=0;
-			tmp_sum_sqr_Q=0;
-			for (int i=0;i<CN0_ESTIMATION_SAMPLES;i++)
-			{
-				tmp_abs_I=std::abs(d_P_I_buffer[i]);
-				tmp_abs_Q=std::abs(d_P_Q_buffer[i]);
-				Psig+=tmp_abs_I;
-				Ptot+=d_P_I_buffer[i]*d_P_I_buffer[i]+d_P_Q_buffer[i]*d_P_Q_buffer[i];
-				tmp_sum_abs_I+=tmp_abs_I;
-				tmp_sum_abs_Q+=tmp_abs_Q;
-				tmp_sum_sqr_I+=(d_P_I_buffer[i]*d_P_I_buffer[i]);
-				tmp_sum_sqr_Q+=(d_P_Q_buffer[i]*d_P_Q_buffer[i]);
-			}
-			Psig=Psig/(float)CN0_ESTIMATION_SAMPLES;
-			Psig=Psig*Psig;
-			d_SNR_SNV=Psig/(Ptot/(float)CN0_ESTIMATION_SAMPLES-Psig);
-			d_SNR_SNV_dB_Hz=10*log10(d_SNR_SNV)+10*log10(d_fs_in/2)-10*log10(d_code_length);
-			NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q;
-			NBP=tmp_sum_sqr_I-tmp_sum_sqr_Q;
-			d_carrier_lock_test=NBD/NBP;
 			d_cn0_estimation_counter=0;
-
+			d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
-		}
+			d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
 			// ###### TRACKING UNLOCK NOTIFICATION #####
+
 			int tracking_message;
 	        if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
 	        {
@@ -469,58 +359,74 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
 	        	tracking_message=3; //loss of lock
 	        	d_channel_internal_queue->push(tracking_message);
 	        	d_carrier_lock_fail_counter=0;
+	            d_current_prn_length_samples=(int)d_vector_length; //original dsp block length
 	        	d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
 
 	        }
 	        //std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
-
+		}
 		// Output the tracking data to navigation and PVT
 		// Output channel 1: Prompt correlator output Q
-		*out[0]=d_P_Q;
+		*out[0]=d_Prompt.real();
 		// Output channel 2: Prompt correlator output I
-		*out[1]=d_P_I;
+		*out[1]=d_Prompt.imag();
-		// Output channel 3: PRN absolute delay [ms]
+		// Output channel 3: Current tracking time [ms]
 		*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
-
+		// Output channel 4: Carrier accumulated phase
-		// Output channel 4: PRN code error [ms]
+		*out[3]=d_acc_carrier_phase_rad;
-		*out[3]=d_code_phase_ms;//(code_error*1000.0)/d_code_freq;
 
 		if(d_dump) {
 			// MULTIPLEXED FILE RECORDING - Record results to file
-			tmp_E=sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q);
+			float prompt_I;
-			tmp_P=sqrt(d_P_I*d_P_I+d_P_Q*d_P_Q);
+			float prompt_Q;
-			tmp_L=sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q);
+			float tmp_E,tmp_P,tmp_L;
-
+			float tmp_float;
+			prompt_I=d_Prompt.imag();
+			prompt_Q=d_Prompt.real();
+			tmp_E=std::abs<float>(d_Early);
+			tmp_P=std::abs<float>(d_Prompt);
+			tmp_L=std::abs<float>(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));
-				// DLL
+				// PROMPT I and Q (to analyze navigation symbols)
-				d_dump_file.write((char*)&code_error, sizeof(float));
+				d_dump_file.write((char*)&prompt_I, sizeof(float));
-				d_dump_file.write((char*)&code_nco, sizeof(float));
+				d_dump_file.write((char*)&prompt_Q, sizeof(float));
-				//PLL
+				// PRN start sample stamp
+				tmp_float=(float)d_sample_counter;
+				d_dump_file.write((char*)&tmp_float, sizeof(float));
+				// accumulated carrier phase
+				d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
+
+				// carrier and code frequency
+				d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
+				d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
+
+				//PLL commands
 				d_dump_file.write((char*)&carr_error, sizeof(float));
 				d_dump_file.write((char*)&carr_nco, sizeof(float));
 
-				//FREQ AND PHASE
+				//DLL commands
-				d_dump_file.write((char*)&d_code_freq, sizeof(float));
+				d_dump_file.write((char*)&code_error, sizeof(float));
-				d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
+				d_dump_file.write((char*)&code_nco, sizeof(float));
 
-				// PROMPT I and Q (to analyze navigation symbols)
+				// CN0 and carrier lock test
-				d_dump_file.write((char*)&d_P_I, sizeof(float));
+				d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
-				d_dump_file.write((char*)&d_P_Q, sizeof(float));
-
-				// Absolute PRN start sample (MATLAB version)
-				//d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
-				//d_dump_file.write((char*)&d_loops_count, sizeof(unsigned long int));
-				d_dump_file.write((char*)&d_SNR_SNV_dB_Hz, sizeof(float));
 				d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
+
+				// AUX vars (for debug purposes)
+				tmp_float=0.0;
+				d_dump_file.write((char*)&tmp_float, sizeof(float));
+				tmp_float=0.0;
+				d_dump_file.write((char*)&tmp_float, sizeof(float));
 	      	 }
 			  catch (std::ifstream::failure e) {
 				std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
 			  }
 		}
+		// ########## DEBUG OUTPUT
 		// debug: Second counter in channel 0
 		if (d_channel==0)
 		{
@@ -528,45 +434,32 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
 			{
 				d_last_seg=floor(d_sample_counter/d_fs_in);
 				std::cout<<"t="<<d_last_seg<<std::endl;
-                    std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
+				std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
 				std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
 			}
-
-			if (d_sample_counter>round((float)this->d_fs_in*70)){ //stop after some seconds debug only!
-			d_enable_tracking=false;
-			std::cout<<"Stop tracking at sample "<<d_sample_counter<<" and acq at sample "<<d_acq_sample_stamp<<std::endl;
-			if(d_queue != gr_msg_queue_sptr()) {
-					ControlMessageFactory* cmf = new ControlMessageFactory();
-					d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
-					delete cmf;
-					std::cout<<"stop sent from tracking";
-				}
-			}
 		}else
 		{
 			if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
 			{
 				d_last_seg=floor(d_sample_counter/d_fs_in);
-              std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
+				std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
 				std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
 			}
 		}
 	}
-	consume_each(d_blksize); // this is necesary in gr_block derivates
+	consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
-        d_sample_counter+=d_blksize; //count for the processed samples
+        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_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
-	d_code_phase = code_phase;
+	d_acq_code_phase_samples = code_phase;
-	LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_code_phase;
+	LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler) {
-	d_carrier_doppler = doppler;
+	d_acq_carrier_doppler_hz = doppler;
-	d_carr_freq_basis = doppler;
+	LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
-	LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_carrier_doppler;
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite) {
@@ -578,3 +471,13 @@ void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) {
 	d_channel = channel;
 	LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
 }
+
+void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
+{
+    d_acq_sample_stamp = sample_stamp;
+}
+
+void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
+{
+    d_channel_internal_queue = channel_internal_queue;
+}

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h

@@ -42,15 +42,14 @@
 //#include <gnuradio/gr_sync_decimator.h>
 
 #include "gps_sdr_signal_processing.h"
+#include "tracking_2rd_DLL_filter.h"
+#include "tracking_2rd_PLL_filter.h"
 
 #include <queue>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/thread.hpp>
 #include "concurrent_queue.h"
 
-// #include "gps_sdr_structs.h"
-// #include "gps_sdr_channel.h"
-
 class gps_l1_ca_dll_pll_tracking_cc;
 typedef boost::shared_ptr<gps_l1_ca_dll_pll_tracking_cc>
         gps_l1_ca_dll_pll_tracking_cc_sptr;
@@ -59,7 +58,11 @@ gps_l1_ca_dll_pll_tracking_cc_sptr
 gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
                                    long fs_in, unsigned
                                    int vector_length,
-                                   gr_msg_queue_sptr queue, bool dump);
+                                   gr_msg_queue_sptr queue,
+                                   bool dump,
+                                   float pll_bw_hz,
+                                   float dll_bw_hz,
+                                   float early_late_space_chips);
 
 //class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
 class gps_l1_ca_dll_pll_tracking_cc: public gr_block
@@ -71,106 +74,88 @@ private:
     gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
                                        long fs_in, unsigned
                                        int vector_length,
-                                       gr_msg_queue_sptr queue, bool dump);
+                                       gr_msg_queue_sptr queue,
+                                       bool dump,
+                                       float pll_bw_hz,
+                                       float dll_bw_hz,
+                                       float early_late_space_chips);
 
     gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq,
                                   long fs_in, unsigned
-                                  int vector_length, gr_msg_queue_sptr queue,
+                                  int vector_length,
-                                  bool dump);
+                                  gr_msg_queue_sptr queue,
-
+                                  bool dump,
-    void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta,
+                                  float pll_bw_hz,
-                             float k);
+                                  float dll_bw_hz,
-    void update_local_code_refs();
+                                  float early_late_space_chips);
+    void update_local_code();
     void update_local_carrier();
 
-    // class private vars
+    // tracking configuration vars
     gr_msg_queue_sptr d_queue;
     concurrent_queue<int> *d_channel_internal_queue;
     unsigned int d_vector_length;
     bool d_dump;
     unsigned int d_satellite;
-    unsigned int d_channel; //?
+    unsigned int d_channel;
     int d_last_seg;
     long d_if_freq;
     long d_fs_in;
-    float d_tau1_code;
-    float d_tau2_code;
 
-    float d_tau1_carr;
+    float d_early_late_spc_chips;
-    float d_tau2_carr;
 
-    float d_early_late_spc;
+    float d_code_phase_step_chips;
-    float d_pdi_code;
-    float d_pdi_carr;
-    float d_dllnoisebandwidth;
-    float d_dlldampingratio;
-
-    float d_pllnoisebandwidth;
-    float d_plldampingratio;
-
-    unsigned int d_code_length;
-    unsigned int d_blk_size;
-    float d_code_phase_step;
 
     gr_complex* d_ca_code;
 
     gr_complex* d_early_code;
     gr_complex* d_late_code;
     gr_complex* d_prompt_code;
-
     gr_complex* d_carr_sign;
-    gr_complex* d_bb_sign;
 
-    float d_code_freq;
+	gr_complex d_Early;
-    float d_rem_code_phase;
+	gr_complex d_Prompt;
+	gr_complex d_Late;
 
-    float d_rem_carr_phase;
+	// remaining code phase and carrier phase between tracking loops
+    float d_rem_code_phase_samples;
+    float d_next_rem_code_phase_samples;
+    float d_rem_carr_phase_rad;
 
-    float d_old_code_nco;
+    // PLL and DLL filter library
-    float d_old_code_error;
+    tracking_2rd_DLL_filter d_code_loop_filter;
+    tracking_2rd_PLL_filter d_carrier_loop_filter;
 
-    float d_old_carr_nco;
+    // acquisition
-    float d_old_carr_error;
+    float d_acq_code_phase_samples;
+    float d_acq_carrier_doppler_hz;
 
-    float d_code_phase;
+    // tracking vars
-    float d_carrier_doppler;
+    float d_code_freq_hz;
-    float d_carr_freq_basis;
+    float d_carrier_doppler_hz;
+    float d_acc_carrier_phase_rad;
 
-    //std::complex<double> d_E;
+    //PRN period in samples
-    //std::complex<double> d_P;
+	int d_current_prn_length_samples;
-    //std::complex<double> d_L;
+	int d_next_prn_length_samples;
-    float d_E_I;
-    float d_E_Q;
-    float d_P_I;
-    float d_P_Q;
-    float d_L_I;
-    float d_L_Q;
-
-    float d_absolute_code_phase_samples;
-    float d_code_phase_ms;
-
-    unsigned int d_blksize;
 
+	//processing samples counters
     unsigned long int d_sample_counter;
     unsigned long int d_acq_sample_stamp;
-    unsigned long int d_loops_count;
 
     // CN0 estimation and lock detector
     int d_cn0_estimation_counter;
-    float* d_P_I_buffer;
+    gr_complex* d_Prompt_buffer;
-    float* d_P_Q_buffer;
     float d_carrier_lock_test;
-    float d_SNR_SNV;
+    float d_CN0_SNV_dB_Hz;
-    float d_SNR_MM;
-    float d_SNR_SNV_dB_Hz;
-
     float 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;
 
@@ -183,16 +168,8 @@ public:
     void set_acq_code_phase(float code_phase);
     void set_acq_doppler(float doppler);
     void start_tracking();
-
+    void set_acq_sample_stamp(unsigned long int sample_stamp);
-    void set_acq_sample_stamp(unsigned long int sample_stamp)
+    void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
-    {
-        d_acq_sample_stamp = sample_stamp;
-    }
-
-    void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
-    {
-        d_channel_internal_queue = channel_internal_queue;
-    }
 
     /*!
      * \brief just like gr_block::general_work, only this arranges to call consume_each for you

src/algorithms/tracking/gnuradio_blocks/jamfile.jam

@@ -1,3 +1,4 @@
 project : build-dir ../../../../build ;
 
 obj gps_l1_ca_dll_pll_tracking_cc : gps_l1_ca_dll_pll_tracking_cc.cc ;
+obj gps_l1_ca_dll_fll_pll_tracking_cc : gps_l1_ca_dll_fll_pll_tracking_cc.cc ;

src/algorithms/tracking/jamfile.jam

@@ -1,2 +1,3 @@
+build-project libs ;
 build-project adapters ;
 build-project gnuradio_blocks ;

src/algorithms/tracking/libs/CN_estimators.cc

@@ -0,0 +1,141 @@
+/*!
+ * \file CN_estimators.cc
+ * \brief Library with a set of Carrier to Noise estimators and lock detectors.
+ * SNV_CN0 is a Carrier-to-Noise (CN0) estimator based on the Signal-to-Noise Variance (SNV) estimator [1].
+ *
+ * Carrier lock detector using normalised estimate of the cosine
+ * of twice the carrier phase error [2].
+ *
+ *
+ * [1] Marco Pini, Emanuela Falletti and Maurizio Fantino, "Performance
+ * Evaluation of C/N0 Estimators using a Real Time GNSS Software Receiver,"
+ * IEEE 10th International Symposium on Spread Spectrum Techniques and
+ * Applications, pp.28-30, August 2008.
+ *
+ *
+ *
+ * [2] Van Dierendonck, A.J. (1996), Global Positioning System: Theory and
+ * Applications,
+ * Volume I, Chapter 8: GPS Receivers, AJ Systems, Los Altos, CA 94024.
+ * Inc.: 329-407.
+ *
+ *
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 "CN_estimators.h"
+#include "GPS_L1_CA.h"
+#include <gnuradio/gr_complex.h>
+#include <math.h>
+
+/*!
+ * Signal-to-Noise (SNR) (\f$\rho\f$) estimator using the Signal-to-Noise Variance (SNV) estimator:
+ * \f{equation}
+ * 	\hat{\rho}=\frac{\hat{P}_s}{\hat{P}_n}=\frac{\hat{P}_s}{\hat{P}_{tot}-\hat{P}_s},
+ * \f}
+ *  where \f$\hat{P}_s=\left(\frac{1}{N}\sum^{N-1}_{i=0}|Re(Pc(i))|\right)^2\f$ is the estimation of the signal power,
+ * \f$\hat{P}_{tot}=\frac{1}{N}\sum^{N-1}_{i=0}|Pc(i)|^2\f$ is the estimator of the total power, \f$|\cdot|\f$ is the absolute value,
+ * \f$Re(\cdot)\f$ stands for the real part of the value, and \f$Pc(i)\f$ is the prompt correlator output for the sample index i.
+ *
+ * The SNR value is converted to CN0 [dB-Hz], taking to account the receiver bandwidth and the PRN code gain, using the following formula:
+ * \f{equation}
+ * 	CN0_{dB}=10*log(\hat{\rho})+10*log(\frac{f_s}{2})-10*log(L_{PRN}),
+ * \f}
+ * where \f$f_s\f$ is the sampling frequency and \f$L_{PRN}\f$ is the PRN sequence length.
+ */
+float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in)
+{
+	// estimate CN0 using buffered values
+	   // MATLAB CODE
+	   //Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2;
+	   //Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2);
+	   //SNR_SNV(count)=Psig/(Ptot-Psig);
+	   //CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
+	float SNR, SNR_dB_Hz;
+	float tmp_abs_I,tmp_abs_Q;
+	float Psig,Ptot;
+	//float M2,M4;
+	Psig=0;
+	Ptot=0;
+	for (int i=0;i<length;i++)
+	{
+		tmp_abs_I=std::abs(Prompt_buffer[i].imag());
+		tmp_abs_Q=std::abs(Prompt_buffer[i].real());
+		Psig+=tmp_abs_I;
+		Ptot+=Prompt_buffer[i].imag()*Prompt_buffer[i].imag()+Prompt_buffer[i].real()*Prompt_buffer[i].real();
+	}
+	Psig=Psig/(float)length;
+	Psig=Psig*Psig;
+	SNR=Psig/(Ptot/(float)length-Psig);
+	SNR_dB_Hz=10*log10(SNR)+10*log10(fs_in/2)-10*log10(GPS_L1_CA_CODE_LENGTH_CHIPS);
+	return SNR_dB_Hz;
+}
+
+/*!
+ * The Carrier Phase Lock Detector block uses the normalised estimate of the cosine of twice the carrier phase error is given by
+ * \f{equation}
+ * 	C2\phi=\frac{NBD}{NBP},
+ * \f}
+ *  where \f$NBD=(\sum^{N-1}_{i=0}|Im(Pc(i))|)^2+(\sum^{N-1}_{i=0}|Re(Pc(i))|)^2\f$,
+ *  \f$NBP=\sum^{N-1}_{i=0}Im(Pc(i))^2-\sum^{N-1}_{i=0}Re(Pc(i))^2\f$, and
+ *  \f$Pc(i)\f$ is the prompt correlator output for the sample index i.
+ */
+
+float carrier_lock_detector(gr_complex* Prompt_buffer, int length)
+{
+	/*!
+	 * \todo Code lock detector
+	 */
+	// estimate using buffered values
+	   // MATLAB CODE
+	   // lock detector operation
+	   //NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2;
+	   //NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
+	   //LOCK(count)=NBD/NBP;
+	float tmp_abs_I,tmp_abs_Q;
+	float tmp_sum_abs_I,tmp_sum_abs_Q;
+	float tmp_sum_sqr_I,tmp_sum_sqr_Q;
+	tmp_sum_abs_I=0;
+	tmp_sum_abs_Q=0;
+	tmp_sum_sqr_I=0;
+	tmp_sum_sqr_Q=0;
+	float NBD,NBP;
+	for (int i=0;i<length;i++)
+	{
+		tmp_abs_I=std::abs(Prompt_buffer[i].imag());
+		tmp_abs_Q=std::abs(Prompt_buffer[i].real());
+		tmp_sum_abs_I+=tmp_abs_I;
+		tmp_sum_abs_Q+=tmp_abs_Q;
+		tmp_sum_sqr_I+=(Prompt_buffer[i].imag()*Prompt_buffer[i].imag());
+		tmp_sum_sqr_Q+=(Prompt_buffer[i].real()*Prompt_buffer[i].real());
+	}
+	NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q;
+	NBP=tmp_sum_sqr_I-tmp_sum_sqr_Q;
+	return NBD/NBP;
+
+}
+

src/algorithms/tracking/libs/CN_estimators.h

@@ -0,0 +1,42 @@
+/*!
+ * \file CN_estimators.h
+ * \brief Library with a set of Carrier to Noise estimators and lock detectors
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Library with a set of Carrier to Noise estimators and lock detectors
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 CN_ESTIMATORS_H_
+#define CN_ESTIMATORS_H_
+
+#include <gnuradio/gr_complex.h>
+
+float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in);
+
+float carrier_lock_detector(gr_complex* Prompt_buffer, int length);
+
+#endif

src/algorithms/tracking/libs/jamfile.jam

@@ -0,0 +1,7 @@
+project : build-dir ../../../../build ;
+
+obj tracking_discriminators : tracking_discriminators.cc ;
+obj CN_estimators : CN_estimators.cc ;
+obj tracking_FLL_PLL_filter	: tracking_FLL_PLL_filter.cc ;
+obj tracking_2rd_PLL_filter	: tracking_2rd_PLL_filter.cc ;
+obj tracking_2rd_DLL_filter	: tracking_2rd_DLL_filter.cc ;

src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc

@@ -0,0 +1,81 @@
+/*!
+ * \file tracking_2rd_DLL_filter.cc
+ * \brief Class that implements 2 order DLL filter for code tracking loop.
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements 2 order PLL filter for code tracking loop. The algorithm is described in [1]
+ *
+ * [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, Applied and Numerical Harmonic Analysis.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 "tracking_2rd_DLL_filter.h"
+
+
+void tracking_2rd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
+	// Solve natural frequency
+	float Wn;
+	Wn = lbw*8*zeta / (4*zeta*zeta + 1);
+	// solve for t1 & t2
+	*tau1 = k / (Wn * Wn);
+	*tau2 = (2.0 * zeta) / Wn;
+}
+
+void tracking_2rd_DLL_filter::set_DLL_BW(float dll_bw_hz)
+{
+	//Calculate filter coefficient values
+	d_dllnoisebandwidth=dll_bw_hz;
+	calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
+}
+void tracking_2rd_DLL_filter::initialize(float d_acq_code_phase_samples)
+{
+    // code tracking loop parameters
+    d_old_code_nco   = 0.0;
+    d_old_code_error = 0.0;
+
+}
+
+float tracking_2rd_DLL_filter::get_code_nco(float DLL_discriminator)
+{
+	float code_nco;
+	code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(DLL_discriminator - d_old_code_error) + DLL_discriminator * (d_pdi_code/d_tau1_code);
+	d_old_code_nco   = code_nco;
+	d_old_code_error = DLL_discriminator; //[chips]
+	return code_nco;
+}
+
+tracking_2rd_DLL_filter::tracking_2rd_DLL_filter ()
+{
+	d_pdi_code = 0.001;// Summation interval for code
+	d_dlldampingratio=0.7;
+}
+
+tracking_2rd_DLL_filter::~tracking_2rd_DLL_filter ()
+{
+
+}

src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h

@@ -0,0 +1,61 @@
+/*!
+ * \file tracking_2rd_DLL_filter.h
+ * \brief Class that implements 2 order DLL filter for code tracking loop.
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements 2 order PLL filter for code tracking loop. The algorithm is described in [1]
+ *
+ * [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, Applied and Numerical Harmonic Analysis.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 TRACKING_2RD_DLL_FILTER_H_
+#define TRACKING_2RD_DLL_FILTER_H_
+
+class tracking_2rd_DLL_filter
+{
+private:
+    // PLL filter parameters
+    float d_tau1_code;
+    float d_tau2_code;
+    float d_pdi_code;
+    float d_dllnoisebandwidth;
+    float d_dlldampingratio;
+
+    float d_old_code_error;
+    float d_old_code_nco;
+
+    void calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k);
+public:
+	void set_DLL_BW(float dll_bw_hz);
+	void initialize(float d_acq_code_phase_samples);
+	float get_code_nco(float DLL_discriminator);
+	tracking_2rd_DLL_filter();
+	~tracking_2rd_DLL_filter();
+};
+
+#endif

src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc

@@ -0,0 +1,80 @@
+/*!
+ * \file tracking_2rd_PLL_filter.cc
+ * \brief Class that implements 2 order PLL filter for tracking carrier loop.
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements 2 order PLL filter for tracking carrier loop. The algorithm is described in [1]
+ *
+ * [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, Applied and Numerical Harmonic Analysis.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 "tracking_2rd_PLL_filter.h"
+
+
+void tracking_2rd_PLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
+	// Solve natural frequency
+	float Wn;
+	Wn = lbw*8*zeta / (4*zeta*zeta + 1);
+	// solve for t1 & t2
+	*tau1 = k / (Wn * Wn);
+	*tau2 = (2.0 * zeta) / Wn;
+}
+
+void tracking_2rd_PLL_filter::set_PLL_BW(float pll_bw_hz)
+{
+	//Calculate filter coefficient values
+	d_pllnoisebandwidth=pll_bw_hz;
+	calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
+}
+void tracking_2rd_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
+{
+    // carrier/Costas loop parameters
+    d_old_carr_nco   = 0.0;
+    d_old_carr_error = 0.0;
+}
+
+float tracking_2rd_PLL_filter::get_carrier_nco(float PLL_discriminator)
+{
+	float carr_nco;
+	carr_nco = d_old_carr_nco+(d_tau2_carr/d_tau1_carr)*(PLL_discriminator - d_old_carr_error) + PLL_discriminator * (d_pdi_carr/d_tau1_carr);
+	d_old_carr_nco   = carr_nco;
+	d_old_carr_error = PLL_discriminator;
+	return carr_nco;
+}
+
+tracking_2rd_PLL_filter::tracking_2rd_PLL_filter ()
+{
+	//--- PLL variables --------------------------------------------------------
+	d_pdi_carr = 0.001;// Summation interval for carrier
+	d_plldampingratio=0.65;
+}
+
+tracking_2rd_PLL_filter::~tracking_2rd_PLL_filter ()
+{
+
+}

src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h

@@ -0,0 +1,62 @@
+/*!
+ * \file tracking_2rd_PLL_filter.h
+ * \brief Class that implements 2 order PLL filter for tracking carrier loop
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements 2 order PLL filter for tracking carrier loop. The algorithm is described in [1]
+ *
+ * [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, Applied and Numerical Harmonic Analysis.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 TRACKING_2RD_PLL_FILTER_H_
+#define TRACKING_2RD_PLL_FILTER_H_
+
+class tracking_2rd_PLL_filter
+{
+private:
+    // PLL filter parameters
+    float d_tau1_carr;
+    float d_tau2_carr;
+    float d_pdi_carr;
+
+    float d_pllnoisebandwidth;
+    float d_plldampingratio;
+
+    float d_old_carr_error;
+    float d_old_carr_nco;
+
+    void calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k);
+public:
+	void set_PLL_BW(float pll_bw_hz);
+	void initialize(float d_acq_carrier_doppler_hz);
+	float get_carrier_nco(float PLL_discriminator);
+	tracking_2rd_PLL_filter();
+	~tracking_2rd_PLL_filter();
+};
+
+#endif

src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc

@@ -0,0 +1,121 @@
+/*!
+ * \file tracking_FLL_PLL_filter.cc
+ * \brief Class that implements hybrid FLL and PLL filter for tracking carrier loop
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements hybrid FLL and PLL filter for tracking carrier loop
+ * Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 "tracking_FLL_PLL_filter.h"
+#include <iostream>
+
+void tracking_FLL_PLL_filter::set_params(float fll_bw_hz,float pll_bw_hz, int order)
+{
+    /*!
+     * Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
+     */
+	d_order=order;
+    if (d_order==3)
+    {
+    	/*!
+    	 *  3rd order PLL with 2nd order FLL assist
+    	 */
+        d_pll_b3 = 2.400;
+        d_pll_a3 = 1.100;
+        d_pll_a2 = 1.414;
+        d_pll_w0p = pll_bw_hz/0.7845;
+        d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
+        d_pll_w0p3 = d_pll_w0p2*d_pll_w0p;
+
+        d_pll_w0f = fll_bw_hz/0.53;
+        d_pll_w0f2 = d_pll_w0f*d_pll_w0f;
+    }else
+    {
+    	/*!
+    	 * 2rd order PLL with 1st order FLL assist
+    	*/
+        d_pll_a2 = 1.414;
+        d_pll_w0p = pll_bw_hz/0.53;
+        d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
+        d_pll_w0f = fll_bw_hz/0.25;
+    }
+
+}
+void tracking_FLL_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
+{
+	if (d_order==3)
+	{
+		d_pll_x 		= 2.0*d_acq_carrier_doppler_hz;
+		d_pll_w	= 0;
+	}else{
+		d_pll_w 		= d_acq_carrier_doppler_hz;
+		d_pll_x 		= 0;
+	}
+	std::cout<<" d_pll_x init = "<<d_pll_x<<"\r\n";
+}
+
+float tracking_FLL_PLL_filter::get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s)
+{
+	float carrier_error_hz;
+	if (d_order==3)
+	{
+    	/*!
+    	 *  3rd order PLL with 2nd order FLL assist
+    	 */
+		d_pll_w = d_pll_w + correlation_time_s * (d_pll_w0p3 * PLL_discriminator + d_pll_w0f2 * FLL_discriminator);
+	    d_pll_x = d_pll_x + correlation_time_s * (0.5*d_pll_w + d_pll_a2 * d_pll_w0f * FLL_discriminator + d_pll_a3 * d_pll_w0p2 * PLL_discriminator);
+		carrier_error_hz  = 0.5*d_pll_x + d_pll_b3 * d_pll_w0p * PLL_discriminator;
+	}else
+	{
+    	/*!
+    	 * 2rd order PLL with 1st order FLL assist
+    	*/
+	   float pll_w_new;
+	   pll_w_new = d_pll_w + PLL_discriminator*d_pll_w0p2*correlation_time_s + FLL_discriminator*d_pll_w0f*correlation_time_s ;
+	   carrier_error_hz = 0.5*(pll_w_new + d_pll_w)+d_pll_a2 * d_pll_w0p*PLL_discriminator;
+	   d_pll_w =pll_w_new;
+	   /*std::cout<<" d_pll_w = "<<carrier_error_hz<<
+			   ", pll_w_new = "<<pll_w_new
+			   <<", PLL_discriminator=" <<PLL_discriminator
+			   <<" FLL_discriminator ="<<FLL_discriminator
+			   <<" correlation_time_s = "<<correlation_time_s<<"\r\n";*/
+	}
+
+	return carrier_error_hz;
+
+}
+
+tracking_FLL_PLL_filter::tracking_FLL_PLL_filter ()
+{
+}
+
+tracking_FLL_PLL_filter::~tracking_FLL_PLL_filter ()
+{
+
+}
+

src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h

@@ -0,0 +1,59 @@
+/*!
+ * \file tracking_FLL_PLL_filter.h
+ * \brief Class that implements hybrid FLL and PLL filter for tracking carrier loop
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Class that implements hybrid FLL and PLL filter for tracking carrier loop
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 TRACKING_FLL_PLL_FILTER_H_
+#define TRACKING_FLL_PLL_FILTER_H_
+
+class tracking_FLL_PLL_filter
+{
+private:
+    // FLL + PLL filter parameters
+	int d_order;
+	float d_pll_w;
+	float d_pll_w0p3;
+	float d_pll_w0f2;
+	float d_pll_x;
+	float d_pll_a2;
+	float d_pll_w0f;
+	float d_pll_a3;
+	float d_pll_w0p2;
+	float d_pll_b3;
+	float d_pll_w0p;
+public:
+	void set_params(float fll_bw_hz,float pll_bw_hz, int order);
+	void initialize(float d_acq_carrier_doppler_hz);
+	float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s);
+	tracking_FLL_PLL_filter();
+	~tracking_FLL_PLL_filter();
+};
+
+#endif

src/algorithms/tracking/libs/tracking_discriminators.cc

@@ -0,0 +1,97 @@
+/*!
+ * \file tracking_discriminators.cc
+ * \brief Library with a set of code tracking and carrier tracking discriminators that is used by the tracking algorithms
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 "tracking_discriminators.h"
+#include <math.h>
+
+//  All the outputs are in RADIANS
+/*!
+ * FLL four quadrant arctan discriminator:
+ * \f{equation}
+ * 	\frac{\phi_2-\phi_1}{t_2-t1}=\frac{ATAN2(cross,dot)}{t_1-t_2},
+ * \f}
+ * where \f$cross=I_{PS1}Q_{PS2}-I_{PS2}Q_{PS1}\f$ and \f$dot=I_{PS1}I_{PS2}+Q_{PS1}Q_{PS2}\f$,
+ * \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_1\f$, and
+ * \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second].
+ */
+
+float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2)
+{
+	float cross,dot;
+	dot=prompt_s1.imag()*prompt_s2.imag()+prompt_s1.real()*prompt_s2.real();
+	cross=prompt_s1.imag()*prompt_s2.real()-prompt_s2.imag()*prompt_s1.real();
+	return atan2(cross,dot)/(t2-t1);
+}
+
+/*!
+ * PLL four quadrant arctan discriminator:
+ * \f{equation}
+ * 	\phi=ATAN2(Q_{PS},I_{PS}),
+ * \f}
+ * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
+ */
+float pll_four_quadrant_atan(gr_complex prompt_s1)
+{
+	return atan2(prompt_s1.real(),prompt_s1.imag());
+}
+
+/*!
+ * PLL Costas loop two quadrant arctan discriminator:
+ * \f{equation}
+ * 	\phi=ATAN\left(\frac{Q_{PS}}{I_{PS}}\right),
+ * \f}
+ * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
+ */
+
+float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
+{
+	if (prompt_s1.imag()!=0.0)
+	{
+		return atan(prompt_s1.real()/prompt_s1.imag());
+	}else{
+		return 0;
+	}
+}
+/*!
+ * DLL Noncoherent Early minus Late envelope normalized discriminator:
+ * \f{equation}
+ * 	error=\frac{E-L}{E+L},
+ * \f}
+ * where \f$E=\sqrt{I_{ES}^2,Q_{ES}^2}\f$ is the Early correlator output absolute value and
+ * \f$L=\sqrt{I_{LS}^2,Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
+ */
+float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
+{
+	float P_early, P_late;
+	P_early=std::abs(early_s1);
+	P_late=std::abs(late_s1);
+	return (P_early-P_late)/((P_early+P_late));
+}

src/algorithms/tracking/libs/tracking_discriminators.h

@@ -0,0 +1,47 @@
+/*!
+ * \file tracking_discriminators.h
+ * \brief Library with a set of code tracking and carrier tracking disctiminators
+ * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Library with a set of code tracking and carrier tracking disctiminators that is used by the tracking algorithms
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2011  (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 TRACKING_DISCRIMINATORS_H_
+#define TRACKING_DISCRIMINATORS_H_
+
+#include <gnuradio/gr_complex.h>
+
+float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2);
+
+float pll_four_quadrant_atan(gr_complex prompt_s1);
+
+float pll_cloop_two_quadrant_atan(gr_complex prompt_s1);
+
+float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1);
+
+
+#endif

src/core/receiver/gnss_block_factory.cc

@@ -55,6 +55,7 @@
 #include "gps_l1_ca_pcps_acquisition.h"
 #include "gps_l1_ca_tong_pcps_acquisition.h"
 #include "gps_l1_ca_dll_pll_tracking.h"
+#include "gps_l1_ca_dll_fll_pll_tracking.h"
 #include "gps_l1_ca_telemetry_decoder.h"
 #include "gps_l1_ca_observables.h"
 
@@ -262,6 +263,11 @@ GNSSBlockInterface* GNSSBlockFactory::GetBlock(
         block = new GpsL1CaDllPllTracking(configuration, role, in_streams,
                 out_streams, queue);
     }
+    else if (implementation.compare("GPS_L1_CA_DLL_FLL_PLL_Tracking") == 0)
+    {
+        block = new GpsL1CaDllFllPllTracking(configuration, role, in_streams,
+                out_streams, queue);
+    }
     else if (implementation.compare("GPS_L1_CA_Telemetry_Decoder") == 0)
     {
         block = new GpsL1CaTelemetryDecoder(configuration, role, in_streams,

src/core/system_parameters/GPS_L1_CA.h

@@ -43,11 +43,19 @@
 const float GPS_C_m_s= 299792458.0;    // The speed of light, [m/ms]
 const float GPS_STARTOFFSET_ms= 68.802; //[ms] Initial sign. travel time
 const float GPS_PI = 3.1415926535898; // Pi used in the GPS coordinate system
+// carrier and code frequencies
+const float GPS_L1_FREQ_HZ	= 1.57542e9;
+const float GPS_L2_FREQ_HZ	= 1.22760e9;
+const float GPS_L1_CA_CODE_RATE_HZ = 1.023e6;
+const float GPS_L1_CA_CODE_LENGTH_CHIPS	=	1023.0;
+
+
 //-- Constants for satellite position calculation -------------------------
 const double  OMEGA_EARTH_DOT = 7.2921151467e-5;  // Earth rotation rate, [rad/s]
 const double  GM            = 3.986005e14;      // Universal gravitational constant times the mass of the Earth, [m^3/s^2]
 const double  F              = -4.442807633e-10; // Constant, [sec/(meter)^(1/2)]
 
+
 // NAVIGATION MESSAGE DEMODULATION AND DECODING
 
 #define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}

src/main/jamfile.jam

@@ -30,7 +30,14 @@ exe mercurio : main.cc
 ../algorithms/telemetry_decoder/gnuradio_blocks//gps_l1_ca_telemetry_decoder_cc
 ../algorithms/telemetry_decoder/libs//gps_l1_ca_subframe_fsm
 ../algorithms/tracking/adapters//gps_l1_ca_dll_pll_tracking
+../algorithms/tracking/adapters//gps_l1_ca_dll_fll_pll_tracking
 ../algorithms/tracking/gnuradio_blocks//gps_l1_ca_dll_pll_tracking_cc
+../algorithms/tracking/gnuradio_blocks//gps_l1_ca_dll_fll_pll_tracking_cc
+../algorithms/tracking/libs//tracking_discriminators
+../algorithms/tracking/libs//CN_estimators
+../algorithms/tracking/libs//tracking_FLL_PLL_filter
+../algorithms/tracking/libs//tracking_2rd_PLL_filter
+../algorithms/tracking/libs//tracking_2rd_DLL_filter
 ../core/libs//INIReader
 ../core/libs//ini
 ../core/libs//string_converter

src/main/main.cc

@@ -43,7 +43,10 @@
 
 using google::LogMessage;
 
-// TODO: make this queue generic for all the GNSS systems (javi)
+/*!
+ * \todo  make this queue generic for all the GNSS systems (javi)
+ */
+
 /*!
  * \brief Concurrent queue that communicates the Telemetry Decoder
  * to the Observables modules