Added J.Arribas contributions: PVT with basic least squares and rinex 2.1 output is now enabled, tracking channels now estimate the CN0 and performs a basic carrier lock detector and returns to acquisition if the tracking loss the lock.

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

conf/mercurio.conf

@@ -7,11 +7,11 @@ ControlThread.wait_for_flowgraph=false
 
 ;######### SIGNAL_SOURCE CONFIG ############
 SignalSource.implementation=File_Signal_Source
-;SignalSource.filename=/media/My Passport/spirent scenario 2/data/sc2_d8.dat
+;SignalSource.filename=/media/DATALOGGER/signals/spirent scenario 2/data/sc2_d8.dat
-;SignalSource.filename=/home/luis/Project/signals/GPS_cap1.dat
+;SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
-SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
+SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
 SignalSource.item_type=gr_complex
-SignalSource.sampling_frequency=8000000
+SignalSource.sampling_frequency=4000000
 SignalSource.samples=0
 SignalSource.repeat=false
 SignalSource.dump=false
@@ -20,58 +20,72 @@ SignalSource.enable_throttle_control=true
 ;######### SIGNAL_CONDITIONER CONFIG ############
 SignalConditioner.implementation=Pass_Through
 SignalConditioner.item_type=gr_complex
-SignalConditioner.sample_freq_in=8000000
+SignalConditioner.sample_freq_in=4000000
-SignalConditioner.sample_freq_out=8000000
+SignalConditioner.sample_freq_out=4000000
 SignalConditioner.dump=false
 
 ;######### CHANNELS CONFIGURATION CONFIG ############
-Channels.count=1
+Channels.count=6
 
 ;######### ACQUISITION CONFIG ############
 
 Acquisition.dump=false
 Acquisition.dump_filename=./acq_dump.dat
 Acquisition.item_type=gr_complex
-Acquisition.fs_in=8000000
+Acquisition.fs_in=4000000
 Acquisition.if=0
 Acquisition.sampled_ms=1
 
 ;######### ACQUISITION 0 CONFIG ############
 Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
-Acquisition0.threshold=0.006
+Acquisition0.threshold=0.06
 Acquisition0.doppler_max=10000
-Acquisition0.doppler_step=250
+Acquisition0.doppler_step=500
-Acquisition0.satellite=1
+Acquisition0.satellite=11
-Acquisition0.repeat_satellite=true
+Acquisition0.repeat_satellite=false
 
 ;######### ACQUISITION 1 CONFIG ############
-Acquisition1.implementation=GPS_L1_CA_GPS_SDR_Acquisition
+Acquisition1.implementation=GPS_L1_CA_PCPS_Acquisition
-Acquisition1.threshold=30
+Acquisition1.threshold=0.06
 Acquisition1.doppler_max=10000
-Acquisition1.doppler_step=250
+Acquisition1.doppler_step=500
-Acquisition1.satellite=16
+Acquisition1.repeat_satellite=false
-Acquisition1.repeat_satellite=true
 
 ;######### ACQUISITION 2 CONFIG ############
-Acquisition2.implementation=GPS_L1_CA_TONG_PCPS_Acquisition
+Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
-Acquisition2.threshold=
+Acquisition2.threshold=0.06
 Acquisition2.doppler_max=10000
-Acquisition2.doppler_step=250
+Acquisition2.doppler_step=500
-Acquisition2.satellite=21
+Acquisition2.repeat_satellite=false
-Acquisition2.repeat_satellite=true
-
 
 ;######### ACQUISITION 3 CONFIG ############
 Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
-Acquisition3.threshold=105000
+Acquisition3.threshold=0.06
+Acquisition3.doppler_max=10000
+Acquisition3.doppler_step=500
+Acquisition3.repeat_satellite=false
+
+;######### ACQUISITION 3 CONFIG ############
+Acquisition4.implementation=GPS_L1_CA_PCPS_Acquisition
+Acquisition4.threshold=0.06
+Acquisition4.doppler_max=10000
+Acquisition4.doppler_step=500
+Acquisition4.repeat_satellite=false
+
+;######### ACQUISITION 3 CONFIG ############
+Acquisition5.implementation=GPS_L1_CA_PCPS_Acquisition
+Acquisition5.threshold=0.06
+Acquisition5.doppler_max=10000
+Acquisition5.doppler_step=500
+Acquisition5.repeat_satellite=false
 
 
 	
 ;######### TRACKING CONFIG ############
 Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
 Tracking.item_type=gr_complex
-Tracking.vector_length=8000
+Tracking.vector_length=4000
-Tracking.fs_in=8000000
+Tracking.fs_in=4000000
 Tracking.if=0
 Tracking.dump=true
 Tracking.dump_filename=./trk_dump.dat
@@ -82,7 +96,7 @@ TelemetryDecoder.item_type=gr_complex
 
 ;######### OBSERVABLES CONFIG ############
 Observables.implementation=GPS_L1_CA_Observables
-Observables.fs_in=8000000;
+Observables.fs_in=4000000;
 
 ;######### PVT CONFIG ############
 PVT.implementation=Pass_Through

jamroot.jam

@@ -8,10 +8,12 @@ lib gnuradio-usrp ;
 lib profiler ;
 lib gsl ;
 lib gslcblas ;
+lib itpp ;
 
 project : requirements
 <define>OMNITHREAD_POSIX
-<cxxflags>"-std=c++0x"
+<cxxflags>"-std=c++0x `pkg-config --cflags itpp`"
+<linkflags>"`pkg-config --libs itpp`"
 # <include>src/utils
 # <include>src/utils/INIReader
 <include>src/algorithms/acquisition/adapters

src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc

@@ -195,6 +195,7 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
         int doppler;
         unsigned int indext = 0;
         float magt = 0.0;
+        float tmp_magt = 0.0;
         const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
         bool positive_acquisition = false;
         int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
@@ -215,8 +216,8 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
         {
             d_input_power += std::abs(in[i]) * std::abs(in[i]);
         }
-        d_input_power = d_input_power / ((float)d_fft_size
+        //d_input_power = d_input_power / ((float)d_fft_size * (float)d_fft_size);
-                * (float)d_fft_size);
+        d_input_power = d_input_power / (float)d_fft_size;
 
         // 2º- Doppler frequency search loop
         for (doppler = (int)(-d_doppler_max); doppler < (int)d_doppler_max; doppler
@@ -238,11 +239,23 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
             }
             d_ifft->execute();
 
-            x86_gr_complex_mag(d_ifft->get_outbuf(), d_fft_size); // d_ifft->get_outbuf()=|abs(·)|^2 and the array is converted from CPX->Float
+            // ASM accelerated version
-            x86_float_max((float*)d_ifft->get_outbuf(), &indext, &magt,
+            //x86_gr_complex_mag(d_ifft->get_outbuf(), d_fft_size); // d_ifft->get_outbuf()=|abs(·)|^2 and the array is converted from CPX->Float
-                    d_fft_size); // find max of |abs(·)|^2 -> index and magt
+            //x86_float_max((float*)d_ifft->get_outbuf(), &indext, &magt,
+            //        d_fft_size); // find max of |abs(·)|^2 -> index and magt
 
-            magt = magt / (float)d_fft_size;
+            // C++ version
+            indext=0;
+            magt=0;
+            for (i = 0; i < d_fft_size; i++)
+            {
+				tmp_magt=std::norm(d_ifft->get_outbuf()[i]);
+				if (tmp_magt>magt){
+					magt=tmp_magt;
+					indext=i;
+				}
+            }
+            //magt = magt / (float)d_fft_size;
             // Record results to files
             if (d_dump)
             {

src/algorithms/channel/adapters/channel.cc

@@ -128,6 +128,8 @@ void Channel::connect(gr_top_block_sptr top_block)
     top_block->connect(trk_->get_right_block(), 0, nav_->get_left_block(), 0); // channel 1
     top_block->connect(trk_->get_right_block(), 1, nav_->get_left_block(), 1); // channel 2
     top_block->connect(trk_->get_right_block(), 2, nav_->get_left_block(), 2); // channel 3
+    top_block->connect(trk_->get_right_block(), 3, nav_->get_left_block(), 3); // channel 4
+    top_block->connect(trk_->get_right_block(), 4, nav_->get_left_block(), 4); // channel 5
     DLOG(INFO) << "tracking -> telemetry_decoder";
 
     connected_ = true;

src/algorithms/observables/adapters/gps_l1_ca_observables.cc

@@ -45,13 +45,6 @@
 #include <glog/log_severity.h>
 #include <glog/logging.h>
 
-/* The extern keyword declares a variable or function and specifies
- * that it has external linkage (its name is visible from files other
- * than the one in which it's defined). When modifying a variable,
- * extern specifies that the variable has static duration (it is allocated
- * when the program begins and deallocated when the program ends).
- * The variable is defined in main.cc
- */
 extern concurrent_queue<gps_navigation_message> global_gps_nav_msg_queue;
 
 using google::LogMessage;

src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc

@@ -52,18 +52,34 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
   d_dump = dump;
   d_nchannels = nchannels;
   d_rinex_printer.set_headers("GNSS-SDR"); //TODO: read filename from config
+  d_ls_pvt=new gps_l1_ca_ls_pvt(nchannels);
+  d_ephemeris_clock_s=0.0;
+  if (d_dump==true)
+  {
+      std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
+      d_dump_filename_str<<"./data/obs.dat"; //TODO: get filename and path from config in the adapter
+      d_dump_filename=d_dump_filename_str.str();
+      d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
+  }
 
 }
 
 gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc() {
 
+    delete d_ls_pvt;
 }
 
-bool pairCompare( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
+bool pairCompare_min( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
 {
-  return a.second.last_preamble_index < b.second.last_preamble_index;
+  return (a.second.preamble_delay_ms+a.second.prn_delay_ms) < (b.second.preamble_delay_ms+b.second.prn_delay_ms);
 }
 
+bool pairCompare_max( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
+{
+    return (a.second.preamble_delay_ms+a.second.prn_delay_ms) > (b.second.preamble_delay_ms+b.second.prn_delay_ms);
+}
+
+
 int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
     gr_vector_const_void_star &input_items,	gr_vector_void_star &output_items) {
 
@@ -71,7 +87,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
 
   // CONSTANTS TODO: place in a file
   const float code_length_s=0.001; //1 ms
-  const float C_m_ms= GPS_C_m_s/1000;    // The speed of light, [m/ms]
+  const float C_m_ms= GPS_C_m_s/1000.0;    // The speed of light, [m/ms]
 
   const float nav_sol_period_ms=1000;
   //--- Find number of samples per spreading code ----------------------------
@@ -84,46 +100,60 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
 
   map<int,float> pseudoranges;
   map<int,float>::iterator pseudoranges_iter;
+  map<int,float> pseudoranges_dump;
 
+  float min_preamble_delay_ms;
+  float max_preamble_delay_ms;
+  bool  flag_valid_pseudoranges=false;
 
-  unsigned long int min_preamble_index;
+  float pseudoranges_timestamp_ms;
-
-  float prn_delay_ms;
   float traveltime_ms;
   float pseudorange_m;
 
+  int pseudoranges_reference_sat_ID=0;
+
   for (unsigned int i=0; i<d_nchannels ; i++)
     {
     if (in[i][0].valid_word) //if this channel have valid word
       {
-      gps_words.insert(pair<int,gnss_synchro>(in[i][0].satellite_PRN,in[i][0])); //record the word structure in a map for pseudoranges
+      gps_words.insert(pair<int,gnss_synchro>(in[i][0].channel_ID,in[i][0])); //record the word structure in a map for pseudoranges
       }
     }
 
   if(gps_words.size()>0)
     {
-    // find the minimum index (nearest satellite, will be the reference)
-    gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare);
-    min_preamble_index=gps_words_iter->second.last_preamble_index;
 
-    //compute the pseudoranges
+      // find the minimum index (nearest satellite, will be the reference)
-    for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
+      gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_min);
-      {
+      min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms; //[ms]
-      prn_delay_ms=gps_words_iter->second.prn_delay/(float)samples_per_code;
+      pseudoranges_timestamp_ms=min_preamble_delay_ms; //save the shortest pseudorange timestamp to compute the RINEX timestamp
-      traveltime_ms=(float)(1000*(gps_words_iter->second.last_preamble_index-min_preamble_index))/(float)samples_per_code+GPS_STARTOFFSET_ms+prn_delay_ms;
+      pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN;
-      //cout<<"traveltime ms"<<gps_words_iter->first<<" ="<<traveltime_ms<<endl;
+      gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_max);
-      pseudorange_m=traveltime_ms*C_m_ms;
+      max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms]
-      pseudoranges.insert(pair<int,float>(gps_words_iter->first,pseudorange_m)); //record the preamble index in a map
-      }
 
+      if ((max_preamble_delay_ms-min_preamble_delay_ms)<70) flag_valid_pseudoranges=true;
+
+
+      //compute the pseudoranges
+      for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
+        {
+        std::cout<<"prn_delay="<<gps_words_iter->second.prn_delay_ms<<std::endl;
+        traveltime_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms-min_preamble_delay_ms+GPS_STARTOFFSET_ms; //[ms]
+        pseudorange_m=traveltime_ms*C_m_ms; // [m]
+        pseudoranges.insert(pair<int,float>(gps_words_iter->second.satellite_PRN,pseudorange_m)); //record the preamble index in a map
+        if (d_dump==true)
+        {
+            pseudoranges_dump.insert(pair<int,float>(gps_words_iter->second.channel_ID,pseudorange_m));
+        }
+        }
     // write the pseudoranges to RINEX OBS file
     // 1- need a valid clock
-    if (d_last_nav_msg.d_satellite_PRN>0)
+    if (flag_valid_pseudoranges==true and d_last_nav_msg.d_satellite_PRN>0)
       {
-        std::cout<<"d_inter_frame_sec_counter="<<d_inter_frame_sec_counter<<std::endl;
+        //d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges);
-        d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_inter_frame_sec_counter,pseudoranges);
+        // compute on the fly PVT solution
+        d_ls_pvt->get_PVT(pseudoranges,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0);
       }
-    d_inter_frame_sec_counter+=((float)NAVIGATION_OUTPUT_RATE_MS)/1000.0; //TODO: synchronize the gps time of the ephemeris with the obs
     }
 
   //debug
@@ -134,29 +164,36 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
     {
     cout<<"Pseudoranges =("<<pseudoranges_iter->first<<","<<pseudoranges_iter->second<<")"<<endl;
     }
-
-
   gps_navigation_message nav_msg;
   if (d_nav_queue->try_pop(nav_msg)==true)
     {
-
+    cout<<"New ephemeris record has arrived from SAT ID "<<nav_msg.d_satellite_PRN<<endl;
-    cout<<"New ephemeris record has arrived!"<<endl;
-    cout<<"d_channel_ID="<<nav_msg.d_channel_ID<<endl;
-    cout<<"d_satellite_PRN="<<nav_msg.d_satellite_PRN<<endl;
-    cout<<"d_satpos_X="<<nav_msg.d_satpos_X<<endl;
-    cout<<"d_satpos_Y="<<nav_msg.d_satpos_Y<<endl;
-    cout<<"d_satpos_Z="<<nav_msg.d_satpos_Z<<endl;
-    cout<<"d_master_clock="<<nav_msg.d_master_clock<<endl;
-    cout<<"d_satClkCorr="<<nav_msg.d_satClkCorr<<endl;
-    cout<<"d_dtr="<<nav_msg.d_dtr<<endl;
-
-    // write ephemeris to RINES NAV file
-    // TODO: check operation ok
     d_last_nav_msg=nav_msg;
-    d_inter_frame_sec_counter=0; //reset the interframe seconds counter
+    d_ls_pvt->d_ephemeris[nav_msg.d_channel_ID]=nav_msg;
-    d_rinex_printer.LogRinex2Nav(nav_msg);
+    // **** update pseudoranges clock ****
+    if (nav_msg.d_satellite_PRN==pseudoranges_reference_sat_ID)
+    {
+        d_ephemeris_clock_s=d_last_nav_msg.d_TOW;
+        d_ephemeris_timestamp_ms=d_last_nav_msg.d_subframe1_timestamp_ms;
+    }
+    // **** write ephemeris to RINES NAV file
+    //d_rinex_printer.LogRinex2Nav(nav_msg);
     }
 
+  if (d_dump==true)
+  {
+      float tmp_float=0.0;
+      for (int i=0;i<d_nchannels;i++)
+      {
+        pseudoranges_iter=pseudoranges_dump.find(i);
+        if (pseudoranges_iter!=pseudoranges_dump.end())
+        {
+            d_dump_file.write((char*)&pseudoranges_iter->second, sizeof(float));
+        }else{
+            d_dump_file.write((char*)&tmp_float, sizeof(float));
+        }
+      }
+  }
   consume_each(1); //one by one
   return 0;
 }

src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h

@@ -24,6 +24,7 @@
 #include "gps_navigation_message.h"
 
 #include "rinex_2_1_printer.h"
+#include "gps_l1_ca_ls_pvt.h"
 
 #include "GPS_L1_CA.h"
 
@@ -56,10 +57,14 @@ private:
 
   rinex_printer d_rinex_printer; // RINEX printer class
 
-  double d_inter_frame_sec_counter; // counter for seconds between GPS frames
-
   gps_navigation_message d_last_nav_msg; //last navigation message
 
+  double d_ephemeris_clock_s;
+  double d_ephemeris_timestamp_ms;
+
+  gps_l1_ca_ls_pvt *d_ls_pvt;
+
+
 public:
 
   ~gps_l1_ca_observables_cc ();

src/algorithms/observables/libs/gps_l1_ca_ls_pvt.cc

@@ -0,0 +1,316 @@
+
+/**
+ * Copyright notice
+ */
+
+/**
+ * Author: Javier Arribas, 2011. jarribas(at)cttc.es
+ */
+
+#include <itpp/itbase.h>
+#include <itpp/stat/misc_stat.h>
+#include <itpp/base/matfunc.h>
+
+using namespace itpp;
+
+#include "gps_l1_ca_ls_pvt.h"
+
+gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels)
+{
+    // init empty ephemerids for all the available GNSS channels
+    d_nchannels=nchannels;
+    d_ephemeris=new gps_navigation_message[nchannels];
+}
+
+gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
+{
+    delete d_ephemeris;
+}
+
+vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, vec X_sat) {
+    /*
+     %E_R_CORR  Returns rotated satellite ECEF coordinates due to Earth
+     %rotation during signal travel time
+     %
+     %X_sat_rot = e_r_corr(traveltime, X_sat);
+     %
+     %   Inputs:
+     %       travelTime  - signal travel time
+     %       X_sat       - satellite's ECEF coordinates
+     %
+     %   Outputs:
+     %       X_sat_rot   - rotated satellite's coordinates (ECEF)
+     */
+
+    double Omegae_dot = 7.292115147e-5; //  rad/sec
+
+    //--- Find rotation angle --------------------------------------------------
+    double omegatau;
+    omegatau = Omegae_dot * traveltime;
+
+    //--- Make a rotation matrix -----------------------------------------------
+    mat R3=zeros(3,3);
+    R3(0, 0)= cos(omegatau);
+    R3(0, 1)= sin(omegatau);
+    R3(0, 2)= 0.0;
+    R3(1, 0)=-sin(omegatau);
+    R3(1, 1)= cos(omegatau);
+    R3(1, 2)=0.0;
+    R3(2, 0)= 0.0;
+    R3(2, 1)= 0.0;
+    R3(2, 2)= 1;
+    //--- Do the rotation ------------------------------------------------------
+    vec X_sat_rot;
+    X_sat_rot = R3 * X_sat;
+    return X_sat_rot;
+
+}
+vec gps_l1_ca_ls_pvt::leastSquarePos(mat satpos, vec obs, mat w) {
+    //Function calculates the Least Square Solution.
+
+    //pos= leastSquarePos(satpos, obs, w);
+
+    //   Inputs:
+    //       satpos      - Satellites positions (in ECEF system: [X; Y; Z;]
+    //       obs         - Observations - the pseudorange measurements to each
+    //                   satellite
+    //       w           - weigths vector
+
+    //   Outputs:
+    //       pos         - receiver position and receiver clock error
+    //                   (in ECEF system: [X, Y, Z, dt])
+
+    //////////////////////////////////////////////////////////////////////////
+    //       el          - Satellites elevation angles (degrees)
+    //       az          - Satellites azimuth angles (degrees)
+    //       dop         - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
+
+    //=== Initialization =======================================================
+    int nmbOfIterations = 10; // TODO: include in config
+
+    //double dtr = GPS_PI / 180.0;
+
+    int nmbOfSatellites;
+    nmbOfSatellites = satpos.cols();
+    vec pos = "0.0 0.0 0.0 0.0";
+    mat A;
+    mat omc;
+    mat az;
+    mat el;
+    A=zeros(nmbOfSatellites,4);
+    omc=zeros(nmbOfSatellites,1);
+    az=zeros(1,nmbOfSatellites);
+    el=zeros(1,nmbOfSatellites);
+    for (int i = 0; i < nmbOfSatellites; i++) {
+        for (int j = 0; j < 4; j++) {
+            A.set(i, j, 0.0);
+        }
+        omc(i, 0)=0.0;
+        az(0, i)=0.0;
+    }
+    el = az;
+    mat X = satpos;
+
+    vec Rot_X;
+    double rho2;
+    double traveltime;
+    double trop;
+    mat mat_tmp;
+    vec x;
+    //=== Iteratively find receiver position ===================================
+    for (int iter = 0; iter < nmbOfIterations; iter++) {
+        for (int i = 0; i < nmbOfSatellites; i++) {
+            if (iter == 0) {
+                //--- Initialize variables at the first iteration --------------
+                Rot_X=X.get_col(i);
+                trop = 0.0;
+            } else {
+                //--- Update equations -----------------------------------------
+                rho2 = (X(0, i) - pos(0))*(X(0, i) - pos(0))  + (X(1, i) - pos(1))*(X(1, i) - pos(1))+ (X(2,i) - pos(2))*(X(2,i) - pos(2));
+                traveltime = sqrt(rho2) / GPS_C_m_s;
+                //--- Correct satellite position (do to earth rotation) --------
+                Rot_X = e_r_corr(traveltime, X.get_col(i));
+
+                //--- Find the elevation angel of the satellite ----------------
+                //[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
+
+            }
+
+            //--- Apply the corrections ----------------------------------------
+            omc(i) = (obs(i) - norm(Rot_X - pos.left(3)) - pos(4) - trop);
+            //--- Construct the A matrix ---------------------------------------
+            A.set(i, 0, (-(Rot_X(0) - pos(0))) / obs(i));
+            A.set(i, 1, (-(Rot_X(1) - pos(1))) / obs(i));
+            A.set(i, 2, (-(Rot_X(2) - pos(2))) / obs(i));
+            A.set(i, 3, 1.0);
+        }
+
+
+        // These lines allow the code to exit gracefully in case of any errors
+        //if (rank(A) != 4) {
+        //  pos.clear();
+        //  return pos;
+        //}
+
+        //--- Find position update ---------------------------------------------
+        x = ls_solve_od(w*A,w*omc);
+
+        //--- Apply position update --------------------------------------------
+        pos = pos + x;
+
+    }
+    return pos;
+}
+
+void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_current_time)
+{
+    std::map<int,float>::iterator pseudoranges_iter;
+    mat satpos;
+    mat W=eye(d_nchannels); //channels weights matrix
+    vec obs=zeros(d_nchannels); // pseudoranges observation vector
+    satpos=zeros(3,d_nchannels); //satellite positions matrix
+    int valid_obs=0; //valid observations counter
+    for (int i=0; i<d_nchannels; i++)
+    {
+        if (d_ephemeris[i].satellite_validation()==true)
+        {
+            pseudoranges_iter=pseudoranges.find(d_ephemeris[i].d_satellite_PRN);
+            if (pseudoranges_iter!=pseudoranges.end())
+            {
+                W(i,i)=1; // TODO: Place here the satellite CN0 (power level, or weight factor)
+                // compute the GPS master clock
+                d_ephemeris[i].master_clock(GPS_current_time);
+                // compute the satellite current ECEF position
+                d_ephemeris[i].satpos();
+                // compute the clock error including relativistic effects
+                d_ephemeris[i].relativistic_clock_correction(GPS_current_time);
+                satpos(0,i)=d_ephemeris[i].d_satpos_X;
+                satpos(1,i)=d_ephemeris[i].d_satpos_Y;
+                satpos(2,i)=d_ephemeris[i].d_satpos_Z;
+                obs(i)=pseudoranges_iter->second+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
+                valid_obs++;
+            }else{
+                // no valid pseudorange for the current channel
+                W(i,i)=0; // channel de-activated
+                obs(i)=1; // to avoid algorithm problems (divide by zero)
+            }
+        }else{
+            // no valid ephemeris for the current channel
+            W(i,i)=0; // channel de-activated
+            obs(i)=1; // to avoid algorithm problems (divide by zero)
+        }
+    }
+    std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
+    if (valid_obs>=4)
+    {
+        vec mypos;
+        mypos=leastSquarePos(satpos,obs,W);
+        std::cout << "Position ("<<GPS_current_time<<") ECEF = " << mypos << std::endl;
+        cart2geo(mypos(0), mypos(1), mypos(2), 4);
+        std::cout << "Position ("<<GPS_current_time<<") Lat = " << d_latitude_d << " Long ="<< d_longitude_d <<" Height="<<d_height_m<< std::endl;
+    }
+}
+void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
+{
+    //function [phi, lambda, h] = cart2geo(X, Y, Z, i)
+    //CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
+    //coordinates (phi, lambda, h) on a selected reference ellipsoid.
+    //
+    //[phi, lambda, h] = cart2geo(X, Y, Z, i);
+    //
+    //   Choices i of Reference Ellipsoid for Geographical Coordinates
+    //             0. International Ellipsoid 1924
+    //             1. International Ellipsoid 1967
+    //             2. World Geodetic System 1972
+    //             3. Geodetic Reference System 1980
+    //             4. World Geodetic System 1984
+
+    double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
+    double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
+
+    double lambda;
+    lambda = atan2(Y,X);
+    double ex2;
+    ex2 = (2-f[elipsoid_selection])*f[elipsoid_selection]/((1-f[elipsoid_selection])*(1-f[elipsoid_selection]));
+    double c;
+    c = a[elipsoid_selection]*sqrt(1+ex2);
+    double phi;
+    phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection])));
+
+    double h = 0.1;
+    double oldh = 0;
+    double N;
+    int iterations = 0;
+    do{
+       oldh = h;
+       N = c/sqrt(1+ex2*(cos(phi)*cos(phi)));
+       phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection])*f[elipsoid_selection]*N/(N+h)))));
+       h = sqrt(X*X+Y*Y)/cos(phi)-N;
+       iterations = iterations + 1;
+       if (iterations > 100)
+       {
+           std::cout<<"Failed to approximate h with desired precision. h-oldh= "<<h-oldh<<std::endl;
+           break;
+       }
+    }while (abs(h-oldh) > 1.0e-12);
+    d_latitude_d = phi*180.0/GPS_PI;
+    d_longitude_d = lambda*180/pi;
+    d_height_m = h;
+}
+
+//void gps_l1_ca_ls_pvt::topocent(traveltime, X_sat)
+//{
+    /*
+%function [Az, El, D] = topocent(X, dx)
+%TOPOCENT  Transformation of vector dx into topocentric coordinate
+%          system with origin at X.
+%          Both parameters are 3 by 1 vectors.
+%
+%[Az, El, D] = topocent(X, dx);
+%
+%   Inputs:
+%       X           - vector origin corrdinates (in ECEF system [X; Y; Z;])
+%       dx          - vector ([dX; dY; dZ;]).
+%
+%   Outputs:
+%       D           - vector length. Units like units of the input
+%       Az          - azimuth from north positive clockwise, degrees
+%       El          - elevation angle, degrees
+
+
+dtr = pi/180;
+
+[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
+
+cl  = cos(lambda * dtr);
+sl  = sin(lambda * dtr);
+cb  = cos(phi * dtr);
+sb  = sin(phi * dtr);
+
+F   = [-sl -sb*cl cb*cl;
+        cl -sb*sl cb*sl;
+        0    cb   sb];
+
+local_vector = F' * dx;
+E   = local_vector(1);
+N   = local_vector(2);
+U   = local_vector(3);
+
+hor_dis = sqrt(E^2 + N^2);
+
+if hor_dis < 1.e-20
+    Az = 0;
+    El = 90;
+else
+    Az = atan2(E, N)/dtr;
+    El = atan2(U, hor_dis)/dtr;
+end
+
+if Az < 0
+    Az = Az + 360;
+end
+
+D   = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
+%%%%%%%%% end topocent.m %%%%%%%%%
+*/
+//}

src/algorithms/observables/libs/gps_l1_ca_ls_pvt.h

@@ -0,0 +1,57 @@
+/**
+ * Copyright notice
+ */
+
+/**
+ * Author: Javier Arribas, 2011. jarribas(at)cttc.es
+ *         Luis Esteve, 2011. luis(at)epsilon-formacion.com
+ */
+
+#ifndef GPS_L1_CA_LS_PVT_H_
+#define GPS_L1_CA_LS_PVT_H_
+
+#include <string>
+#include <iostream>
+#include <sstream>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <time.h>
+#include <math.h>
+#include <map>
+#include <algorithm>
+#include "gps_navigation_message.h"
+#include "GPS_L1_CA.h"
+
+#include <itpp/itbase.h>
+#include <itpp/stat/misc_stat.h>
+#include <itpp/base/matfunc.h>
+
+using namespace itpp;
+
+class gps_l1_ca_ls_pvt
+{
+private:
+    vec leastSquarePos(mat satpos, vec obs, mat w);
+    vec e_r_corr(double traveltime, vec X_sat);
+    //void cart2geo();
+    //void topocent();
+public:
+    int d_nchannels;
+    gps_navigation_message* d_ephemeris;
+    double d_pseudoranges_time_ms;
+    double d_latitude_d;
+    double d_longitude_d;
+    double d_height_m;
+    double d_x_m;
+    double d_y_m;
+    double d_z_m;
+
+    gps_l1_ca_ls_pvt(int nchannels);
+    ~gps_l1_ca_ls_pvt();
+
+    void get_PVT(std::map<int,float> pseudoranges,double GPS_current_time);
+    void cart2geo(double X, double Y, double Z, int elipsoid_selection);
+};
+
+#endif

src/algorithms/observables/libs/jamfile.jam

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

src/algorithms/observables/libs/rinex_2_1_printer.cc

@@ -192,7 +192,7 @@ void rinex_printer::Rinex2ObsHeader()
 
 }
 
-void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interframe_seconds, std::map<int,float> pseudoranges)
+void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double pseudoranges_clock, std::map<int,float> pseudoranges)
 {
 	int ss;
 	char sat_vis[36];
@@ -224,7 +224,7 @@ void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interfram
 		double decimalday,daydecimalhour,decimalhour,decimalmin,decimalsec;
 		double day,hour,minutes,seconds,enterseconds,a;
 		double gpstime;
-		gpstime=nav_msg.d_master_clock;
+		gpstime=pseudoranges_clock; //[s]
 		//calculate date of gps time:
 		//Days & weeks between 00h 1 Jan 1970 and 00h 6 Jan 1980
 		//520 weeks and 12 days.
@@ -233,7 +233,7 @@ void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interfram
 		tmPtr = gmtime(&temps);
 		strftime( cad1, 20, " %y %m %d", tmPtr );
 		strftime( cad2, 20, "  %Y    %m    %d", tmPtr );
-		decimalday=((gpstime+interframe_seconds)/(24*3600));//Dies dins de la semana
+		decimalday=(gpstime/(24*3600));//Dies dins de la semana
 		daydecimalhour=modf(decimalday,&day);//day=#dies sencers, daydecimalhour=porcio de dia
 		daydecimalhour=daydecimalhour*24;//porcio de dia en hores
 		decimalhour=modf(daydecimalhour,&hour);//hour=hora del dia; decimalhour=porcio d'hora

src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc

@@ -45,13 +45,6 @@
 #include <glog/log_severity.h>
 #include <glog/logging.h>
 
-/* The extern keyword declares a variable or function and specifies
- * that it has external linkage (its name is visible from files other
- * than the one in which it's defined). When modifying a variable,
- * extern specifies that the variable has static duration (it is allocated
- * when the program begins and deallocated when the program ends).
- * The variable is defined in main.cc
- */
 extern concurrent_queue<gps_navigation_message> global_gps_nav_msg_queue;
 
 using google::LogMessage;

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc

@@ -47,7 +47,7 @@ void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
 
 gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
     int vector_length, gr_msg_queue_sptr queue, bool dump) :
-    gr_block ("gps_navigation_cc", gr_make_io_signature (3, 3, sizeof(float)),
+    gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(float)),
         gr_make_io_signature(1, 1, sizeof(gnss_synchro))) {
   // initialize internal vars
   d_queue = queue;
@@ -106,10 +106,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
   d_sample_counter++; //count for the processed samples
 
   const float **in = (const float **)  &input_items[0]; //Get the input samples pointer
-  //std::cout<<"CH1="<<in[0][0]; // Q
-  //std::cout<<"CH2="<<in[1][0]; // I
-  //std::cout<<"CH3="<<in[2][0]; // Delay
-  //std::cout<<"N_input_streams="<<ninput_items.size();
 
   // TODO Optimize me!
   //******* preamble correlation ********
@@ -123,13 +119,13 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
   }
 
   //******* frame sync ******************
-  if (abs(corr_value)>=153){
+  if (abs(corr_value)>=160){
     //TODO: Rewrite with state machine
     if (d_stat==0)
       {
       d_GPS_FSM.Event_gps_word_preamble();
       d_preamble_index=d_sample_counter;//record the preamble sample stamp
-      std::cout<<"Pre-detection"<<std::endl;
+      std::cout<<"Pre-detection SAT "<<this->d_satellite+1<<std::endl;
       d_symbol_accumulator=0; //sync the symbol to bits integrator
       d_symbol_accumulator_counter=0;
       d_frame_bit_index=8;
@@ -140,15 +136,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
         if (abs(preamble_diff-6000)<1)
           {
           d_GPS_FSM.Event_gps_word_preamble();
-          d_preamble_index=d_sample_counter;//record the preamble sample stamp
+          d_preamble_index=d_sample_counter;//record the preamble sample stamp (t_P)
+          d_preamble_phase=in[2][0]; //record the PRN start sample index associated to the preamble
+
           if (!d_flag_frame_sync){
             d_flag_frame_sync=true;
-            std::cout<<" Frame sync with phase "<<in[2][0]<<std::endl;
+            std::cout<<" Frame sync SAT "<<this->d_satellite+1<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
           }
           }else
             {
             if (preamble_diff>7000){
-              std::cout<<"lost of frame sync"<<std::endl;
+              std::cout<<"lost of frame sync SAT "<<this->d_satellite+1<<std::endl;
               d_stat=0; //lost of frame sync
               d_flag_frame_sync=false;
             }
@@ -156,6 +154,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
         }
   }
 
+  //******* code error accumulator *****
+  //d_preamble_phase-=in[3][0];
   //******* SYMBOL TO BIT *******
 
   d_symbol_accumulator+=in[1][d_samples_per_bit*8-1]; // accumulate the input value in d_symbol_accumulator
@@ -194,6 +194,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
         }
       if (gps_word_parityCheck(d_GPS_frame_4bytes)) {
         memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4);
+        d_GPS_FSM.d_preamble_time_ms=d_preamble_phase;
         d_GPS_FSM.Event_gps_word_valid();
         d_flag_parity=true;
       }else{
@@ -213,8 +214,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
   if ((d_sample_counter%NAVIGATION_OUTPUT_RATE_MS)==0)
     {
     gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true);
-    gps_synchro.last_preamble_index=d_preamble_index;
+    //gps_synchro.preamble_delay_ms=(float)d_preamble_index;
-    gps_synchro.prn_delay=in[2][0];
+    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.channel_ID=d_channel;
     *out[0]=gps_synchro;

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h

@@ -76,6 +76,8 @@ private:
   int d_satellite;
   int d_channel;
 
+  float d_preamble_phase;
+
   std::string d_dump_filename;
   std::ofstream d_dump_file;
 

src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc

@@ -12,7 +12,7 @@ public:
   // sc::transition(evento,estado_destino)
   typedef sc::transition< Ev_gps_word_preamble, gps_subframe_fsm_S1 > reactions;
   gps_subframe_fsm_S0(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S0 "<<std::endl;
+    //std::cout<<"Enter S0 "<<std::endl;
   }
 };
 struct gps_subframe_fsm_S1: public sc::state<gps_subframe_fsm_S1, GpsL1CaSubframeFsm > {
@@ -21,7 +21,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S2 > > reactions;
 
   gps_subframe_fsm_S1(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S1 "<<std::endl;
+    //std::cout<<"Enter S1 "<<std::endl;
   }
 };
 
@@ -31,7 +31,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S3 > > reactions;
 
   gps_subframe_fsm_S2(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S2 "<<std::endl;
+    //std::cout<<"Enter S2 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(0);
   }
 };
@@ -41,7 +41,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S4 > > reactions;
 
   gps_subframe_fsm_S3(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S3 "<<std::endl;
+    //std::cout<<"Enter S3 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(1);
   }
 };
@@ -51,7 +51,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S5 > > reactions;
 
   gps_subframe_fsm_S4(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S4 "<<std::endl;
+    //std::cout<<"Enter S4 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(2);
   }
 };
@@ -61,7 +61,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S6 > > reactions;
 
   gps_subframe_fsm_S5(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S5 "<<std::endl;
+    //std::cout<<"Enter S5 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(3);
   }
 };
@@ -71,7 +71,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S7 > > reactions;
 
   gps_subframe_fsm_S6(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S6 "<<std::endl;
+    //std::cout<<"Enter S6 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(4);
   }
 };
@@ -81,7 +81,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S8 > > reactions;
 
   gps_subframe_fsm_S7(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S7 "<<std::endl;
+    //std::cout<<"Enter S7 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(5);
   }
 };
@@ -91,7 +91,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S9 > > reactions;
 
   gps_subframe_fsm_S8(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S8 "<<std::endl;
+    //std::cout<<"Enter S8 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(6);
   }
 };
@@ -101,7 +101,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S10 > > reactions;
 
   gps_subframe_fsm_S9(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S9 "<<std::endl;
+    //std::cout<<"Enter S9 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(7);
   }
 };
@@ -111,7 +111,7 @@ public:
   sc::transition< Ev_gps_word_valid, gps_subframe_fsm_S11 > > reactions;
 
   gps_subframe_fsm_S10(my_context ctx): my_base( ctx ){
-    std::cout<<"Enter S10 "<<std::endl;
+    //std::cout<<"Enter S10 "<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(8);
   }
 };
@@ -120,11 +120,11 @@ public:
   typedef sc::transition< Ev_gps_word_preamble, gps_subframe_fsm_S1 > reactions;
 
   gps_subframe_fsm_S11(my_context ctx): my_base( ctx ){
-    std::cout<<"Completed GPS Subframe!"<<std::endl;
+    //std::cout<<"Completed GPS Subframe!"<<std::endl;
     context< GpsL1CaSubframeFsm >().gps_word_to_subframe(9);
     context< GpsL1CaSubframeFsm >().gps_subframe_to_nav_msg(); //decode the subframe
     // DECODE SUBFRAME
-    std::cout<<"Enter S11"<<std::endl;
+    //std::cout<<"Enter S11"<<std::endl;
   }
 };
 
@@ -149,17 +149,14 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
 
   d_nav.d_satellite_PRN=d_satellite_PRN+1;
   d_nav.d_channel_ID=d_channel_ID;
-
+  if (subframe_ID==1) {
+    d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms-6002;
+    std::cout<<"FSM: set subframe1 preamble timestamp for sat "<<d_nav.d_satellite_PRN<<std::endl;
+  }
   //TODO: change to subframe 5
   if (subframe_ID==3) { // if the subframe is the 5th, then
     if (d_nav.satellite_validation()) // if all the satellite ephemeris parameters are good, then
       {
-        // compute the GPS master clock
-        d_nav.master_clock();
-        // compute the satellite current ECEF position
-        d_nav.satpos();
-        // compute the clock error including relativistic effects
-        d_nav.relativistic_clock_correction();
         // Send the procesed satellite ephemeris packet
         d_nav_queue->push(d_nav);
       }

src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h

@@ -61,6 +61,8 @@ public:
   char d_subframe[GPS_SUBFRAME_LENGTH];
   char d_GPS_frame_4bytes[GPS_WORD_LENGTH];
 
+  double d_preamble_time_ms;
+
   void gps_word_to_subframe(int position);
   void gps_subframe_to_nav_msg();
 

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -19,12 +19,11 @@
 #include "gps_l1_ca_dll_pll_tracking_cc.h"
 #include "gps_sdr_signal_processing.h"
 
-//#include "gps_sdr_signaldef.h"
 #include "gps_sdr_simd.h"
 #include "gps_sdr_x86.h"
 
 #include "control_message_factory.h"
-
+#include <boost/lexical_cast.hpp>
 #include <iostream>
 #include <sstream>
 #include <cmath>
@@ -37,6 +36,11 @@
 #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_pll_tracking_cc_sptr
@@ -47,11 +51,18 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs
 			fs_in, vector_length, queue, dump));
 }
 
+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
+}
 
 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) :
-		gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
+	    gr_block ("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) {
+	              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;
@@ -61,38 +72,39 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
 	d_vector_length = vector_length;
 
 	// Initialize tracking variables ==========================================
-	//TODO: Include this setting in configuration file
+	/*!
+	 * \todo Include PLL and DLL filter setting in configuration file
+	 */
 
 	//--- DLL variables --------------------------------------------------------
 	d_early_late_spc = 0.5; // Define early-late offset (in chips)
 	d_pdi_code = 0.001;// Summation interval for code
-	d_dllnoisebandwidth=2.0; //Hz
+	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=25;
+	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
 
-    // Get a vector with the C/A code sampled 1x/chip
 	d_code_length=1023;
-	d_ca_code=(gr_complex*)malloc(sizeof(gr_complex)*(d_code_length+2));
+    // Get space for a vector with the C/A code replica sampled 1x/chip
+    d_ca_code=new gr_complex[d_code_length+2];
 
     // Get space for the resampled early / prompt / late local replicas
-    d_early_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
+    d_early_code= new gr_complex[d_vector_length*2];
-    // Get space for the resampled early / prompt / late local replicas
+    d_prompt_code=new gr_complex[d_vector_length*2];
-    d_prompt_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
+    d_late_code=new gr_complex[d_vector_length*2];
-    // Get space for the resampled early / prompt / late local replicas
-    d_late_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
 
-    d_carr_sign=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
+    // space for carrier wipeoff and signal baseband vectors
-    d_bb_sign=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
+    d_carr_sign=new gr_complex[d_vector_length*2];
+    d_bb_sign=new gr_complex[d_vector_length*2];
 
     //--- Perform initializations ------------------------------
 
@@ -114,167 +126,194 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
     d_old_carr_nco   = 0.0;
     d_old_carr_error = 0.0;
 
-    d_absolute_code_phase_chips = 0;
+    d_absolute_code_phase_samples = 0;
 
     // sample synchronization
     d_sample_counter=0;
     d_acq_sample_stamp=0;
 
-    d_dump_filename="./data/trk_epr.dat";
     d_enable_tracking=false;
-	d_last_seg=0;
+    d_pull_in=false;
+    d_last_seg=0;
+
+    d_blksize=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_P_Q_buffer=new float[CN0_ESTIMATION_SAMPLES];
+    d_carrier_lock_test=1;
+    d_SNR_SNV=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;
-	//std::cout<<"Tau1= "<<*tau1<<std::endl;
+}
-	//std::cout<<"Tau2= "<<*tau2<<std::endl;
+
+
+void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
+
+    unsigned long int acq_sample_difference;
+    int trk_corrected_code_phase;
+
+    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));
+
+    if (trk_corrected_code_phase<0)
+    {
+        trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
+    }
+    d_absolute_code_phase_samples=(float)trk_corrected_code_phase;
+
+    // generate local reference ALWAYS starting at chip 1, not corrected
+    code_gen_conplex(&d_ca_code[1],d_satellite,0);
+
+    // Then make it possible to do early and late versions
+    d_ca_code[0]=d_ca_code[1023];
+    d_ca_code[1024]=d_ca_code[1];
+
+    DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
+
+    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 {
+			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";
+		  }
+    	}
+     }
+    d_carrier_lock_fail_counter=0;
+    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;
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::update_local_code_refs()
 {
 	float tcode;
-	float max_tcode;
+	int associated_chip_index;
-	unsigned int i=0;
+	// unified loop for E, P, L code vectors
-	int tmp_index;
+    for (unsigned int i=0;i<d_blksize;i++)
-	float block_correction;
+    {
+        tcode=i*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
+        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 = ceil(fmod(tcode, d_code_length));
+        d_prompt_code[i] = d_ca_code[associated_chip_index];
+        tcode += d_early_late_spc;
+        associated_chip_index = ceil(fmod(tcode, d_code_length));
+        d_late_code[i] = d_ca_code[associated_chip_index];
+    }
 
-//	std::cout<<"d_code_phase_step"<<d_code_phase_step<<std::endl;
+    //**** Option 1: Keep the number of samples per PRN period constant and equal to the nominal value
-//	std::cout<<"d_rem_code_phase"<<d_rem_code_phase<<std::endl;
+    //****           and record the size mismatch in a var: d_rem_code_phase
-//	std::cout<<"d_blk_size"<<d_blk_size<<std::endl;
+    //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;
 
-	// Define index into early code vector
+    //**** Option 2: Each loop, compute the new PRN sequence code length according to the estimated Doppler
-	max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
+    tcode=d_blksize*d_code_phase_step+d_rem_code_phase;
-
+    d_rem_code_phase = tcode - 1023.0; //prompt remaining code phase
-	for (tcode=d_rem_code_phase-d_early_late_spc;tcode<max_tcode;tcode+=d_code_phase_step)
-	{
-		tmp_index=ceil(fmod(tcode,d_code_length));
-		d_early_code[i]=d_ca_code[tmp_index];
-		i++;
-//		if (i==d_vector_length){
-//			std::cout<<"Break "<<std::endl;
-//			break;
-//		}
-	}
-	// Define index into late code vector
-	i=0;
-	max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase+d_early_late_spc;
-	for (tcode=d_rem_code_phase+d_early_late_spc;tcode<max_tcode;tcode+=d_code_phase_step)
-	{
-		tmp_index=ceil(fmod(tcode,d_code_length));
-		d_late_code[i]=d_ca_code[tmp_index];
-		i++;
-	}
-	// Define index into prompt code vector
-	i=0;
-	max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase;
-	for (tcode=d_rem_code_phase;tcode<max_tcode;tcode+=d_code_phase_step)
-	{
-		tmp_index=ceil(fmod(tcode,d_code_length));
-		d_prompt_code[i]=d_ca_code[tmp_index];
-		i++;
-	}
-
-	block_correction=(float)d_blk_size*(1/(float)d_fs_in)-(float)d_code_length*(1/d_code_freq);
-	d_rem_code_phase = (max_tcode + d_code_phase_step - block_correction) - 1023.0;
-	//std::cout<<"d_rem_code_phase="<<d_rem_code_phase<<std::endl;
 }
 
 void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
 {
-		float phase, phase_step;
+        float phase, phase_step;
 
-		phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
+        phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
-		phase=d_rem_carr_phase;
+        phase=d_rem_carr_phase;
-		for(unsigned int i = 0; i < d_vector_length; i++)	{
+        for(unsigned int i = 0; i < d_blksize; i++) {
-			d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
+            d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
-			phase += phase_step;
+            phase += phase_step;
-		}
+        }
-		d_rem_carr_phase=fmod(phase,TWO_PI);
+        d_rem_carr_phase=fmod(phase,TWO_PI);
 }
+
 gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
+    /*!
+     * \todo free memory!!
+     */
 	d_dump_file.close();
-
-}
-
-void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
-	unsigned long int acq_sample_difference;
-	int trk_corrected_code_phase;
-
-	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;
-
-	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));
-
-	if (trk_corrected_code_phase<0)
-	{
-		trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
-	}
-	d_absolute_code_phase_chips=(float)trk_corrected_code_phase;
-
-	/*
-	std::cout<<"Acq sample stamp"<<d_acq_sample_stamp<<std::endl;
-	std::cout<<"Acq - Trk sample difference "<<acq_sample_difference<<std::endl;
-	std::cout<<"Trk local code correction "<<trk_corrected_code_phase<<std::endl;
-	std::cout<<"Rounded"<<round((float)d_code_length*(float)trk_corrected_code_phase/(float)d_vector_length);
-	 */
-	// generate local reference with the acquisition shift corrected
-    code_gen_conplex(&d_ca_code[1],d_satellite,1023-round((float)d_code_length*(float)trk_corrected_code_phase/(float)d_vector_length));
-    // Then make it possible to do early and late versions
-    d_ca_code[0]=d_ca_code[1023];
-    d_ca_code[1024]=d_ca_code[1];
-
-	DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
-	d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
-	d_enable_tracking=true;
-	std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID "<< this->d_satellite << std::endl;
 }
 
 /*! 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_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
 
-	d_sample_counter+=d_vector_length; //count for the processed samples
+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);
+	        d_code_phase_ms=(d_absolute_code_phase_samples*1000.0)/(float)d_fs_in;
+	        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;
+	    }
 		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];
+		const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
 
 		float **out = (float **) &output_items[0];
 
-		// Find the size of a "block" or code period in whole samples
-
 		// Update the phasestep based on code freq (variable) and
 		// sampling frequency (fixed)
-		d_code_phase_step = d_code_freq / d_fs_in;
+		// code_phase_step_per_sample = T_sample/T_chip
+		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]
 
-		// original variable block size (MATLAB)
-		//d_blk_size = ceil(((float)d_code_length-d_rem_code_phase) / d_code_phase_step);
 
-		// fixed block size (Javi)
+		//float rem_code_phase_samples=d_rem_code_phase/d_code_phase_step;
-		d_blk_size = ceil((float)d_code_length*(1/1023000.0)*(float)d_fs_in);
+		//d_absolute_code_phase_samples-=floor(rem_code_phase_samples);
+
+		//d_rem_code_phase=d_code_phase_step*(rem_code_phase_samples-floor(rem_code_phase_samples));
 
 		this->update_local_code_refs();
 		this->update_local_carrier();
@@ -287,7 +326,8 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
 		d_P_Q=0.0;
 		d_L_Q=0.0;
 
-		for(unsigned int i=0;i<d_vector_length;i++) {
+		// 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
@@ -302,11 +342,14 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
 			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) / TWO_PI;
+		carr_error = atan(d_P_Q / d_P_I) / (float)TWO_PI;
 
 		// Implement carrier loop filter and generate NCO command
 
@@ -325,33 +368,102 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
 		// 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;
+		d_old_code_error = code_error; //[chips]
-
-		d_absolute_code_phase_chips+=d_old_code_error; //accumulate the error to get the pseudorange
 
 		// Modify code freq based on NCO command
 		d_code_freq = 1023000 - code_nco;
 
-		// Output the tracking data to navigation and PVT
 
+		d_code_phase_ms+=(1023000/d_code_freq)-1.0;
+
+		/*!
+		 * \todo Code lock detector
+		 */
+
+		// ####### 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_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*std::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;
+
+		}
+		// ###### 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{
+        	d_carrier_lock_fail_counter--;
+        }
+        if (d_carrier_lock_fail_counter>200)
+        {
+        	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 tranking 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;
 		// Output channel 2: Prompt correlator output I
 		*out[1]=d_P_I;
-		// Output channel 3: Prompt correlator output
+		// Output channel 3: PRN absolute delay [ms]
-		*out[2]=d_absolute_code_phase_chips;
+		*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
-
-		//Equivalent to the "absolute spreading code starting position" information of the matlab tracking algorithm
-
-//		std::cout<<"tmp_E= "<<tmp_E<<std::endl;
-//		std::cout<<"tmp_P= "<<tmp_P<<std::endl;
-//		std::cout<<"tmp_L= "<<tmp_L<<std::endl;
-//
-//		std::cout<<"trk pllDiscr carr_error= "<<carr_error<<std::endl;
-//		std::cout<<"trk dllDiscr code error= "<<code_error<<std::endl;
-//		std::cout<<"trk dllDiscrFilt code_nco= "<<code_nco<<std::endl;
-//		std::cout<<"trk pllDiscrFilt carr_nco= "<<carr_nco<<std::endl;
 
+		// Output channel 4: PRN code error [ms]
+		*out[3]=d_code_phase_ms;//(code_error*1000.0)/d_code_freq;
 
 		if(d_dump) {
 			// MULTIPLEXED FILE RECORDING - Record results to file
@@ -359,23 +471,35 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
 			tmp_P=sqrt(d_P_I*d_P_I+d_P_Q*d_P_Q);
 			tmp_L=sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q);
 
-			// EPR
+	      	try {
-			d_dump_file.write((char*)&tmp_E, sizeof(float));
+				// EPR
-			d_dump_file.write((char*)&tmp_P, sizeof(float));
+				d_dump_file.write((char*)&tmp_E, sizeof(float));
-			d_dump_file.write((char*)&tmp_L, sizeof(float));
+				d_dump_file.write((char*)&tmp_P, sizeof(float));
-			// DLL
+				d_dump_file.write((char*)&tmp_L, sizeof(float));
-			d_dump_file.write((char*)&code_error, sizeof(float));
+				// DLL
-			d_dump_file.write((char*)&code_nco, sizeof(float));
+				d_dump_file.write((char*)&code_error, sizeof(float));
-			//PLL
+				d_dump_file.write((char*)&code_nco, sizeof(float));
-			d_dump_file.write((char*)&carr_error, sizeof(float));
+				//PLL
-			d_dump_file.write((char*)&carr_nco, sizeof(float));
+				d_dump_file.write((char*)&carr_error, sizeof(float));
-			//FREQ AND PHASE
+				d_dump_file.write((char*)&carr_nco, sizeof(float));
-			d_dump_file.write((char*)&d_code_freq, sizeof(float));
-			d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
-			// PROMPT I and Q (to analyze navigation symbols)
-			d_dump_file.write((char*)&d_P_I, sizeof(float));
-			d_dump_file.write((char*)&d_P_Q, sizeof(float));
 
+				//FREQ AND PHASE
+				d_dump_file.write((char*)&d_code_freq, sizeof(float));
+				d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
+
+				// PROMPT I and Q (to analyze navigation symbols)
+				d_dump_file.write((char*)&d_P_I, 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));
+	      	 }
+			  catch (std::ifstream::failure e) {
+				std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
+			  }
 		}
 		// debug: Second counter in channel 0
 		if (d_channel==0)
@@ -384,21 +508,33 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
                     {
                     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<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
                     }
-		}
+
-		if (d_sample_counter>round((float)this->d_fs_in*39)){ //stop after some seconds debug only!
+			if (d_sample_counter>round((float)this->d_fs_in*70)){ //stop after some seconds debug only!
-		d_enable_tracking=false;
+			d_enable_tracking=false;
-		std::cout<<"Stop tracking at sample "<<d_sample_counter<<" and acq at sample "<<d_acq_sample_stamp<<std::endl;
+			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()) {
+			if(d_queue != gr_msg_queue_sptr()) {
-				ControlMessageFactory* cmf = new ControlMessageFactory();
+					ControlMessageFactory* cmf = new ControlMessageFactory();
-				d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
+					d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
-				delete cmf;
+					delete cmf;
-				std::cout<<"stop sent from tracking" << std::endl;
+					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<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
+              }
 		}
 	}
-	//consume_each(1); //not necesary for gr_sync_block derivates
+	consume_each(d_blksize); // this is necesary in gr_block derivates
-	return 1; //one group of d_vector_lenght samples at time, and output one set of results ALWAYS even in the case of d_enable_tracking==false
+        d_sample_counter+=d_blksize; //count for the processed samples
+	return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
 }
 
 

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h

@@ -37,7 +37,7 @@
 
 #include <gnuradio/gr_block.h>
 #include <gnuradio/gr_msg_queue.h>
-#include <gnuradio/gr_sync_decimator.h>
+//#include <gnuradio/gr_sync_decimator.h>
 
 #include "gps_sdr_signal_processing.h"
 
@@ -59,7 +59,8 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
                                    int vector_length,
                                    gr_msg_queue_sptr queue, bool dump);
 
-class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
+//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
+class gps_l1_ca_dll_pll_tracking_cc: public gr_block
 {
 
 private:
@@ -143,11 +144,30 @@ private:
     float d_L_I;
     float d_L_Q;
 
-    float d_absolute_code_phase_chips;
+    float d_absolute_code_phase_samples;
+    float d_code_phase_ms;
+
+    unsigned int d_blksize;
 
     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;
+    float* d_P_Q_buffer;
+    float d_carrier_lock_test;
+    float d_SNR_SNV;
+    float d_SNR_MM;
+    float d_SNR_SNV_dB_Hz;
+
+    float d_carrier_lock_threshold;
+
+    int d_carrier_lock_fail_counter;
+
     bool d_enable_tracking;
+    bool d_pull_in;
 
     std::string d_dump_filename;
     std::ofstream d_dump_file;
@@ -181,8 +201,12 @@ public:
     //                  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,
+   //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_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);
 
 };
 

src/core/system_parameters/GPS_L1_CA.h

@@ -40,7 +40,7 @@
 
 
 // SEPARATE FILE GPS.H
-const float GPS_C_m_s= 299792458;    // The speed of light, [m/ms]
+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
 //-- Constants for satellite position calculation -------------------------
@@ -60,8 +60,7 @@ const double  F              = -4.442807633e-10; // Constant, [sec/(meter)^(1/2)
 
 /*! @ingroup GPS_DEFINES
  *  @brief Navigation message bits slice structure: A portion of bits is indicated by
- *  the start position inside the subframe and the length in number of bits
+ *  the start position inside the subframe and the length in number of bits  */
- */
 typedef struct bits_slice{
         int position;
         int length;
@@ -74,12 +73,12 @@ typedef struct bits_slice{
 
 
 /*! @ingroup GPS_DEFINES
- *  @brief Demodulator gnss_synchro structure, used to feed the pseudorange block
+ *  @brief Demodulator gnss_synchro structure, used to feed the pseudorange block */
- */
+
 typedef struct gnss_synchro
 {
-  unsigned long int last_preamble_index;
+  float preamble_delay_ms;
-  float prn_delay;
+  float prn_delay_ms;
   int satellite_PRN;
   int channel_ID;
   bool valid_word;

src/core/system_parameters/gps_navigation_message.cc

@@ -1,6 +1,6 @@
 /*!
  * \file gps_navigation_message.cc
- * \brief  Implementation of GPS L1 C/A navigation message decoding
+ * \brief  Navigation message structure for GPS L1 C/A signal
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
  *
  * -------------------------------------------------------------------------
@@ -30,7 +30,6 @@
 
 #include "gps_navigation_message.h"
 
-//! Sets all the ephemeris parameters to zero
 void gps_navigation_message::reset()
 {
   d_TOW=0;
@@ -65,9 +64,9 @@ void gps_navigation_message::reset()
   d_SV_accuracy=0;
   d_SV_health=0;
   d_TGD=0;
-  d_IODC=0;
+  d_IODC=-1;
   //broadcast orbit 7
-  d_transmission_time=0;
+
   d_fit_interval=0;
   d_spare1=0;
   d_spare2=0;
@@ -89,15 +88,17 @@ void gps_navigation_message::reset()
   // info
   d_channel_ID=0;
   d_satellite_PRN=0;
+
+  // time synchro
+  d_subframe1_timestamp_ms=0;
+
 }
 
-//! Default constructor
 gps_navigation_message::gps_navigation_message()
 {
   reset();
-}
 
-//! Read unsigned values
+}
 unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const bits_slice *slices, int num_of_slices)
 {
   unsigned long int value;
@@ -117,7 +118,6 @@ unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<G
   return value;
 }
 
-//! Read signed values
 signed long int gps_navigation_message::read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const bits_slice *slices, int num_of_slices)
 {
   signed long int value=0;
@@ -145,13 +145,7 @@ signed long int gps_navigation_message::read_navigation_signed(std::bitset<GPS_S
   return value;
 }
 
-/*!
+
- * \brief Accounts for beginning or end of week crossover
- *
- * Inspired in a Matlab function by Kai Borre
- * \param[in] time (in seconds)
- * \result Returns corrected time
- */
 double gps_navigation_message::check_t(double time)
 {
   /*
@@ -163,6 +157,13 @@ corrTime = check_t(time);
 
    Outputs:
        corrTime    - corrected time (seconds)
+
+Kai Borre 04-01-96
+Copyright (c) by Kai Borre
+
+ CVS record:
+ $Id: check_t.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
+==========================================================================
    */
   double corrTime;
   double half_week = 302400;     // seconds
@@ -177,23 +178,21 @@ corrTime = check_t(time);
   return corrTime;
 }
 
-void gps_navigation_message::master_clock()
+void gps_navigation_message::master_clock(double transmitTime)
 {
   double dt;
   double satClkCorr;
   // Find initial satellite clock correction --------------------------------
 
   // --- Find time difference ---------------------------------------------
-  dt = check_t(d_TOW - d_Toc);
+  dt = check_t(transmitTime - d_Toc);
 
   //--- Calculate clock correction ---------------------------------------
   satClkCorr = (d_A_f2 * dt + d_A_f1) * dt +  d_A_f0 - d_TGD;
 
-  d_master_clock = d_TOW - satClkCorr;
+  d_master_clock = transmitTime - satClkCorr;
 }
 
-
-//! Computes satellite position from ephemeris data
 void gps_navigation_message::satpos()
 {
   double tk;
@@ -239,7 +238,7 @@ void gps_navigation_message::satpos()
     E_old   = E;
     E       = M + d_e_eccentricity * sin(E);
     dE      = fmod(E - E_old,2*GPS_PI);
-    std::cout<<"dE="<<dE<<std::endl;
+    //std::cout<<"dE="<<dE<<std::endl;
     if (abs(dE) < 1E-12)
       {
       //Necessary precision is reached, exit from the loop
@@ -289,19 +288,17 @@ void gps_navigation_message::satpos()
   d_satpos_Z = sin(u)*r * sin(i);
 }
 
-void gps_navigation_message::relativistic_clock_correction()
+void gps_navigation_message::relativistic_clock_correction(double transmitTime)
 {
   double dt;
-  // Find initial satellite clock correction --------------------------------
+  // Find final satellite clock correction --------------------------------
 
   // --- Find time difference ---------------------------------------------
-  dt = check_t(d_TOW - d_Toc);
+  dt = check_t(transmitTime - d_Toc);
 
   //Include relativistic correction in clock correction --------------------
   d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 -d_TGD + d_dtr;
 }
-
-//! Subframe decoder
 int gps_navigation_message::subframe_decoder(char *subframe)
 {
   int subframe_ID=0;
@@ -333,7 +330,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     }
    */
   subframe_ID=(int)read_navigation_unsigned(subframe_bits,SUBFRAME_ID,num_of_slices(SUBFRAME_ID));
-  std::cout<<"subframe ID="<<subframe_ID<<std::endl;
+  //std::cout<<"subframe ID="<<subframe_ID<<std::endl;
 
   // Decode all 5 sub-frames
   switch (subframe_ID){
@@ -371,6 +368,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     d_A_f2=d_A_f2*A_F2_LSB;
 
     /* debug print */
+    /*
     std::cout<<"d_TOW="<<d_TOW<<std::endl;
     std::cout<<"GPS week="<<d_GPS_week<<std::endl;
     std::cout<<"SV_accuracy="<<d_SV_accuracy<<std::endl;
@@ -381,7 +379,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     std::cout<<"A_F0="<<d_A_f0<<std::endl;
     std::cout<<"A_F1="<<d_A_f1<<std::endl;
     std::cout<<"A_F2="<<d_A_f2<<std::endl;
-
+*/
     /*
         eph.weekNumber  = bin2dec(subframe(61:70)) + 1024;
         eph.accuracy    = bin2dec(subframe(73:76));
@@ -396,13 +394,10 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     break;
 
   case 2:
-
+    //tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
-    tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
+    //std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
-    tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
-    std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
     // --- It is subframe 2 -------------------------------------
     // It contains first part of ephemeris parameters
-
     d_IODE_SF2=(double)read_navigation_unsigned(subframe_bits,IODE_SF2,num_of_slices(IODE_SF2));
     d_Crs=(double)read_navigation_signed(subframe_bits,C_RS,num_of_slices(C_RS));
     d_Crs=d_Crs*C_RS_LSB;
@@ -422,6 +417,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     d_Toe=d_Toe*T_OE_LSB;
 
     /* debug print */
+    /*
     std::cout<<"d_IODE_SF2="<<d_IODE_SF2<<std::endl;
     std::cout<<"d_Crs="<<d_Crs<<std::endl;
     std::cout<<"d_Delta_n="<<d_Delta_n<<std::endl;
@@ -431,7 +427,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     std::cout<<"d_Cus="<<d_Cus<<std::endl;
     std::cout<<"d_sqrt_A="<<d_sqrt_A<<std::endl;
     std::cout<<"d_Toe="<<d_Toe<<std::endl;
-
+*/
     break;
     /*
         eph.IODE_sf2    = bin2dec(subframe(61:68));
@@ -445,9 +441,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
         eph.t_oe        = bin2dec(subframe(271:286)) * 2^4;
      */
   case 3:
-    tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
+    //tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
-    tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
+    //std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
-    std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
     // --- It is subframe 3 -------------------------------------
     // It contains second part of ephemeris parameters
     d_Cic=(double)read_navigation_signed(subframe_bits,C_IC,num_of_slices(C_IC));
@@ -469,6 +464,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     d_IDOT=d_IDOT*I_DOT_LSB;
 
     /* debug print */
+    /*
     std::cout<<"d_Cic="<<d_Cic<<std::endl;
     std::cout<<"d_OMEGA0="<<d_OMEGA0<<std::endl;
     std::cout<<"d_Cis="<<d_Cis<<std::endl;
@@ -478,6 +474,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     std::cout<<"d_OMEGA_DOT="<<d_OMEGA_DOT<<std::endl;
     std::cout<<"d_IODE_SF3="<<d_IODE_SF3<<std::endl;
     std::cout<<"d_IDOT="<<d_IDOT<<std::endl;
+    */
 
     break;
     /*
@@ -492,9 +489,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
         eph.iDot        = twosComp2dec(subframe(279:292)) * 2^(-43) * gpsPi;
      */
   case 4:
-    tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
+    //tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
-    tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
+    //std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
-    std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
     // --- It is subframe 4 -------------------------------------
     // Almanac, ionospheric model, UTC parameters.
     // SV health (PRN: 25-32)
@@ -502,14 +498,14 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     SV_page=(int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
 
     /* debug print */
+    /*
     std::cout<<"SF4 SV_data_ID="<<SV_data_ID<<std::endl;
     std::cout<<"SF4 SV_page="<<SV_page<<std::endl;
-
+*/
     break;
   case 5:
-    tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
+    //tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
-    tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
+    //std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
-    std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
     //--- It is subframe 5 -------------------------------------
     // SV almanac and health (PRN: 1-24).
     // Almanac reference week number and time.
@@ -517,8 +513,10 @@ int gps_navigation_message::subframe_decoder(char *subframe)
     SV_page=(int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
 
     /* debug print */
+    /*
     std::cout<<"SF5 SV_data_ID="<<SV_data_ID<<std::endl;
     std::cout<<"SF5 SV_page="<<SV_page<<std::endl;
+    */
     break;
   default:
     break;
@@ -534,7 +532,7 @@ bool gps_navigation_message::satellite_validation()
 
   // first step: check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception
   //             and check if the data has been filled (!=0)
-  if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=0)
+  if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=-1)
     {
     flag_data_valid=true;
     }

src/core/system_parameters/gps_navigation_message.h

@@ -1,6 +1,6 @@
 /*!
  * \file gps_navigation_message.h
- * \brief  This class implements GPS L1 C/A navigation message decoding
+ * \brief  Navigation message structure for GPS L1 C/A signal
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
  *
  * -------------------------------------------------------------------------
@@ -28,6 +28,7 @@
  * -------------------------------------------------------------------------
  */
 
+
 #ifndef GPS_NAVIGATION_MESSAGE_H
 #define GPS_NAVIGATION_MESSAGE_H
 
@@ -44,13 +45,6 @@ using namespace boost::assign;
 
 #include "GPS_L1_CA.h"
 
-/*!
- * \brief  This class implements GPS L1 C/A navigation message decoding
- * and computes satellite position
- *
- * It defines the specific GPS navigation (NAV) data structure,
- * as defined in the document Interface Specification IS-GPS-200 Revision E, Appendix II
- */
 class gps_navigation_message
 {
 
@@ -93,7 +87,7 @@ public:
     double d_TGD;
     double d_IODC;
     //broadcast orbit 7
-    double d_transmission_time;
+
     double d_fit_interval;
     double d_spare1;
     double d_spare2;
@@ -118,12 +112,15 @@ public:
     int d_channel_ID;
     int d_satellite_PRN;
 
+    // time synchro
+    double d_subframe1_timestamp_ms; //[ms]
+
     // public functions
     void reset();
     int subframe_decoder(char *subframe);
-    void master_clock();
+    void master_clock(double transmitTime);
     void satpos();
-    void relativistic_clock_correction();
+    void relativistic_clock_correction(double transmitTime);
     bool satellite_validation();
     gps_navigation_message();
 };

src/core/system_parameters/gps_telemetry.cc

@@ -1,7 +1,6 @@
 /*!
  * \file gps_telemetry.cc
- * \brief  Implementation of low-levels functions for GPS L1 C/A
+ * \brief GPS L1 C/A telemetry processing
- * navigation message decoding
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
  *
  * -------------------------------------------------------------------------

src/core/system_parameters/gps_telemetry.h

@@ -1,7 +1,6 @@
 /*!
  * \file gps_telemetry.h
- * \brief  Define low-level constants and functions for GPS L1 C/A
+ * \brief GPS L1 C/A telemetry processing
- * navigation message decoding
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
  *
  * -------------------------------------------------------------------------
@@ -28,7 +27,6 @@
  *
  * -------------------------------------------------------------------------
  */
-
 #ifndef GPS_TELEMETRY_H_
 #define GPS_TELEMETRY_H_
 

src/main/jamfile.jam

@@ -21,6 +21,7 @@ exe mercurio : main.cc
 ../algorithms/observables/adapters//gps_l1_ca_observables
 ../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
 ../algorithms/observables/libs//rinex_2_1_printer
+../algorithms/observables/libs//gps_l1_ca_ls_pvt
 ../algorithms/output_filter/adapters//file_output_filter
 ../algorithms/output_filter/adapters//null_sink_output_filter
 ../algorithms/signal_source/adapters//file_signal_source

src/main/main.cc

@@ -71,5 +71,5 @@ int main(int argc, char** argv)
     control_thread->run();
 
     delete control_thread;
-    google::ShutDownCommandLineFlags();
+    //google::ShutDownCommandLineFlags();
 }