Bug fixes and Code refactoring in telemetry decoder

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc

@@ -110,6 +110,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
     d_channel = 0;
     Prn_timestamp_at_preamble_ms = 0.0;
     flag_PLL_180_deg_phase_locked = false;
+
+    tmp_counter=0;
 }
 
 
@@ -145,6 +147,7 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
 int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
         gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
 {
+
     int corr_value = 0;
     int preamble_diff_ms = 0;
 
@@ -182,7 +185,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
                     d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the preamble sample stamp
                     DLOG(INFO)  << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0);
                     //sync the symbol to bits integrator
-                    d_symbol_accumulator = 0;                    d_symbol_accumulator_counter = 0;
+                    d_symbol_accumulator = 0;
+                    d_symbol_accumulator_counter = 0;
                     d_frame_bit_index = 0;
                     d_stat = 1; // enter into frame pre-detection status
                 }
@@ -331,25 +335,33 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
      //1. Copy the current tracking output
      current_synchro_data = in[0][0];
      //2. Add the telemetry decoder information
-     if (this->d_flag_preamble == true and d_GPS_FSM.d_nav.d_TOW > 0)
+         if (this->d_flag_preamble == true and d_GPS_FSM.d_nav.d_TOW > 0)
-         //update TOW at the preamble instant (todo: check for valid d_TOW)
+             //update TOW at the preamble instant (todo: check for valid d_TOW)
-         // JAVI: 30/06/2014
+             // JAVI: 30/06/2014
-         // TOW, in GPS, is referred to the START of the SUBFRAME, that is, THE FIRST SYMBOL OF THAT SUBFRAME, NOT THE PREAMBLE.
+             // TOW, in GPS, is referred to the START of the SUBFRAME, that is, THE FIRST SYMBOL OF THAT SUBFRAME, NOT THE PREAMBLE.
-         // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_subframe_start.
+             // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_subframe_start.
-         // Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol.
+             // Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol.
-         {
+             {
-             d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
+                 d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW;
-             d_TOW_at_current_symbol = d_TOW_at_Preamble;
+                 Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
-             Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
+                 //std::cout.precision(17);
-             if (flag_TOW_set == false)
+                 //std::cout<<"symbol diff="<<tmp_counter<<" Preable TOW="<<std::fixed<<d_TOW_at_Preamble
-                 {
+                 //        <<" with DeltaTOW="<<d_TOW_at_Preamble-d_TOW_at_current_symbol
-                     flag_TOW_set = true;
+                 //        <<" decoded at "<<Prn_timestamp_at_preamble_ms/1000<<"[s]\r\n";
-                 }
+
-         }
+                 d_TOW_at_current_symbol = d_TOW_at_Preamble;
+                 if (flag_TOW_set == false)
+                     {
+                         flag_TOW_set = true;
+                     }
+
+                 tmp_counter=0;
+             }
      else
-         {
+     {
-             d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
+         tmp_counter++;
-         }
+         d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
+     }
 
      current_synchro_data.d_TOW = d_TOW_at_Preamble;
      current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h

@@ -98,6 +98,9 @@ private:
     double d_symbol_accumulator;
     short int d_symbol_accumulator_counter;
 
+    //debug
+    int tmp_counter;
+
     //bits and frame
     unsigned short int d_frame_bit_index;
     unsigned int d_GPS_frame_4bytes;
@@ -121,8 +124,8 @@ private:
 
     double d_preamble_time_seconds;
 
-    double d_TOW_at_Preamble;
+    long double d_TOW_at_Preamble;
-    double d_TOW_at_current_symbol;
+    long double d_TOW_at_current_symbol;
 
     double Prn_timestamp_at_preamble_ms;
     bool flag_TOW_set;

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -416,6 +416,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
 
             // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample
             current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter+d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+            current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
             current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
             current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
             current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@@ -430,6 +431,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
                 }
 
             current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter+d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+            current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
             current_synchro_data.System = {'G'};
         }
 

src/core/system_parameters/gnss_synchro.h

@@ -58,6 +58,7 @@ public:
     double Carrier_Doppler_hz;      //!< Set by Tracking processing block
     double Carrier_phase_rads;      //!< Set by Tracking processing block
     double Tracking_timestamp_secs; //!< Set by Tracking processing block
+    double Rem_code_phase_secs;     //!< Set by Tracking processing block
 
     bool Flag_valid_symbol_output; //!< Set by Tracking processing block
     int correlation_length_ms; //!< Set by Tracking processing block

src/core/system_parameters/gps_navigation_message.cc

@@ -428,14 +428,13 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
     case 1:
         //--- It is subframe 1 -------------------------------------
         // Compute the time of week (TOW) of the first sub-frames in the array ====
-        // Also correct the TOW. The transmitted TOW is actual TOW of the next
+        // The transmitted TOW is actual TOW of the next subframe
-        // subframe and we need the TOW of the first subframe in this data block
         // (the variable subframe at this point contains bits of the last subframe).
-        //TOW = bin2dec(subframe(31:47)) * 6 - 30;
+        //TOW = bin2dec(subframe(31:47)) * 6;
         d_TOW_SF1 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
         //we are in the first subframe (the transmitted TOW is the start time of the next subframe) !
         d_TOW_SF1 = d_TOW_SF1 * 6;
-        d_TOW = d_TOW_SF1 - 6; // Set transmission time
+        d_TOW = d_TOW_SF1; // Set transmission time
         b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
         b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
         b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@@ -461,7 +460,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
     case 2:  //--- It is subframe 2 -------------------
         d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
         d_TOW_SF2 = d_TOW_SF2 * 6;
-        d_TOW = d_TOW_SF2 - 6; // Set transmission time
+        d_TOW = d_TOW_SF2; // Set transmission time
         b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
         b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
         b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@@ -491,7 +490,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
     case 3: // --- It is subframe 3 -------------------------------------
         d_TOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
         d_TOW_SF3 = d_TOW_SF3 * 6;
-        d_TOW = d_TOW_SF3 - 6; // Set transmission time
+        d_TOW = d_TOW_SF3; // Set transmission time
         b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
         b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
         b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@@ -520,7 +519,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
         int SV_page;
         d_TOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
         d_TOW_SF4 = d_TOW_SF4 * 6;
-        d_TOW = d_TOW_SF4 - 6; // Set transmission time
+        d_TOW = d_TOW_SF4; // Set transmission time
         b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
         b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
         b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@@ -596,7 +595,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
         int SV_page_5;
         d_TOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
         d_TOW_SF5 = d_TOW_SF5 * 6;
-        d_TOW = d_TOW_SF5 - 6; // Set transmission time
+        d_TOW = d_TOW_SF5; // Set transmission time
         b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
         b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
         b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);