Added optimizations to Galileo E1 VEML

Not finished yet...debugging

src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc

@@ -49,7 +49,7 @@
 #include "lock_detectors.h"
 #include "Galileo_E1.h"
 #include "control_message_factory.h"
-
+#include "fxpt64.h"
 
 
 /*!
@@ -255,21 +255,43 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
     rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
     tcode_half_chips = - rem_code_phase_half_chips;
 
-    early_late_spc_samples = std::round(d_early_late_spc_chips / code_phase_step_chips);
-    very_early_late_spc_samples = std::round(d_very_early_late_spc_chips / code_phase_step_chips);
 
-    epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples * 2;
+    int64_t prompt_code_phase_fxp = double_to_fxpt64( tcode_half_chips );
+    // NOTE: TODO:
+    // This is WRONG!!!!! The early should be + and the late should be -
+    // FIXME
+    int64_t very_early_code_phase_fxp = double_to_fxpt64( tcode_half_chips + 2*d_very_early_late_spc_chips );
+    int64_t early_code_phase_fxp = double_to_fxpt64( tcode_half_chips + 2*d_early_late_spc_chips );
+    int64_t late_code_phase_fxp = double_to_fxpt64( tcode_half_chips - 2*d_early_late_spc_chips );
+    int64_t very_late_code_phase_fxp = double_to_fxpt64( tcode_half_chips - 2*d_very_early_late_spc_chips );
 
-    for (int i = 0; i < epl_loop_length_samples; i++)
+    int64_t code_phase_step_fxp = double_to_fxpt64( code_phase_step_half_chips );
+
+    LOG(INFO) << "Initial code phase: " << very_late_code_phase_fxp << ". "
+              << "Integer part: " << (very_late_code_phase_fxp >> 32 ) << ". "
+              << "Increment: " << code_phase_step_fxp << ". "
+              << "As double: " << static_cast< double >( code_phase_step_fxp ) * std::pow( 2, -32 );
+
+    //EPL code generation
+    for (int i = 0; i < d_current_prn_length_samples; i++)
         {
-            associated_chip_index = 2 + std::round(std::fmod(tcode_half_chips - 2 * d_very_early_late_spc_chips, code_length_half_chips));
-            d_very_early_code[i] = d_ca_code[associated_chip_index];
-            tcode_half_chips = tcode_half_chips + code_phase_step_half_chips;
+            d_very_early_code[i] = d_ca_code[ 1 + ( very_early_code_phase_fxp >> 32 ) ];
+            d_early_code[i] = d_ca_code[ 1 + ( early_code_phase_fxp >> 32 ) ];
+            d_prompt_code[i] = d_ca_code[ 1 + ( prompt_code_phase_fxp >> 32 ) ];
+            d_late_code[i] = d_ca_code[ 1 + ( late_code_phase_fxp >> 32 ) ];
+            d_very_late_code[i] = d_ca_code[ 1 + ( very_late_code_phase_fxp >> 32 ) ];
+
+            very_early_code_phase_fxp += code_phase_step_fxp;
+            early_code_phase_fxp += code_phase_step_fxp;
+            prompt_code_phase_fxp += code_phase_step_fxp;
+            late_code_phase_fxp += code_phase_step_fxp;
+            very_late_code_phase_fxp += code_phase_step_fxp;
         }
-    memcpy(d_early_code, &d_very_early_code[very_early_late_spc_samples - early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
-    memcpy(d_prompt_code, &d_very_early_code[very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
-    memcpy(d_late_code, &d_very_early_code[very_early_late_spc_samples + early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
-    memcpy(d_very_late_code, &d_very_early_code[2 * very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
+
+    LOG(INFO) << "Final code phase: " << very_early_code_phase_fxp << ". "
+              << "Integer part: " << (very_early_code_phase_fxp >> 32 ) << ". "
+              << "Increment: " << code_phase_step_fxp << ". "
+              << "As double: " << static_cast< double >( code_phase_step_fxp ) * std::pow( 2, -32 );
 }
 
 
@@ -350,6 +372,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items, gr_vec
             update_local_code();
             update_local_carrier();
 
+
             // perform carrier wipe-off and compute Very Early, Early, Prompt, Late and Very Late correlation
             d_correlator.Carrier_wipeoff_and_VEPL_volk(d_current_prn_length_samples,
                     in,
@@ -365,6 +388,28 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items, gr_vec
                     d_Late,
                     d_Very_Late);
 
+            //gr_complex phase_as_complex( std::cos( d_rem_carr_phase_rad ),
+                        //-std::sin( d_rem_carr_phase_rad ) );
+
+            //double carrier_doppler_inc_rad = 2.0*M_PI*d_carrier_doppler_hz/d_fs_in;
+
+            //gr_complex phase_inc_as_complex( std::cos( carrier_doppler_inc_rad ),
+                    //-std::sin( carrier_doppler_inc_rad ) );
+            //d_correlator.Carrier_rotate_and_VEPL_volk(d_current_prn_length_samples,
+                    //in,
+                    //&phase_as_complex,
+                    //phase_inc_as_complex,
+                    //d_very_early_code,
+                    //d_early_code,
+                    //d_prompt_code,
+                    //d_late_code,
+                    //d_very_late_code,
+                    //d_Very_Early,
+                    //d_Early,
+                    //d_Prompt,
+                    //d_Late,
+                    //d_Very_Late );
+
             // ################## PLL ##########################################################
             // PLL discriminator
             carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GALILEO_TWO_PI;

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -49,15 +49,9 @@
 #include "lock_detectors.h"
 #include "GPS_L1_CA.h"
 #include "control_message_factory.h"
+#include "fxpt64.h"
 
 
-int64_t double_to_fxp64( double in )
-{
-    int64_t out = static_cast< int64_t >( in * std::pow( 2.0, 32.0 ) );
-
-    return out;
-}
-
 /*!
  * \todo Include in definition header file
  */
@@ -286,14 +280,14 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
     rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
     tcode_chips = -rem_code_phase_chips;
 
-    int64_t prompt_code_phase_fxp = double_to_fxp64( tcode_chips );
+    int64_t prompt_code_phase_fxp = double_to_fxpt64( tcode_chips );
     // NOTE: TODO:
     // This is WRONG!!!!! The early should be + and the late should be -
     // FIXME
-    int64_t early_code_phase_fxp = double_to_fxp64( tcode_chips - d_early_late_spc_chips );
-    int64_t late_code_phase_fxp = double_to_fxp64( tcode_chips + d_early_late_spc_chips );
+    int64_t early_code_phase_fxp = double_to_fxpt64( tcode_chips - d_early_late_spc_chips );
+    int64_t late_code_phase_fxp = double_to_fxpt64( tcode_chips + d_early_late_spc_chips );
 
-    int64_t code_phase_step_fxp = double_to_fxp64( code_phase_step_chips );
+    int64_t code_phase_step_fxp = double_to_fxpt64( code_phase_step_chips );
 
     //EPL code generation
     for (int i = 0; i < d_current_prn_length_samples; i++)

src/algorithms/tracking/libs/CMakeLists.txt

@@ -30,9 +30,10 @@ if(ENABLE_CUDA)
 endif(ENABLE_CUDA)
 
 
-set(TRACKING_LIB_SOURCES   
+set(TRACKING_LIB_SOURCES
      correlator.cc
      cpu_multicorrelator.cc
+     fxpt64.cc
      lock_detectors.cc
      tcp_communication.cc
      tcp_packet_data.cc

src/algorithms/tracking/libs/fxpt64.cc

@@ -0,0 +1,43 @@
+/*!
+ * \file fxpt64.cc
+ * \brief Utility functions for fixed point 64 bit arithmetic
+ * \authors <ul>
+ *          <li> Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
+ *          </ul>
+ *
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "fxpt64.h"
+#include <cmath>
+
+int64_t double_to_fxpt64( double in, unsigned int frac_len )
+{
+    int64_t out = static_cast< int64_t >( in * std::pow(2.0, frac_len) );
+
+    return out;
+}
+

src/algorithms/tracking/libs/fxpt64.h

@@ -0,0 +1,40 @@
+/*!
+ * \file fxpt64.h
+ * \brief Utility functions for fixed point 64 bit arithmetic
+ * \authors <ul>
+ *          <li> Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
+ *          </ul>
+ *
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_FXPT64_H_
+#define GNSS_SDR_FXPT64_H_
+#include <cstdint>
+
+int64_t double_to_fxpt64( double in, unsigned int frac_len = 32 );
+
+#endif