[hamradio-commits] [gnss-sdr] 84/251: Updates in the GPS L2C CNAV telemetry decoder. Migrating code from SBAS decoder. Viterbi is working now and CRC check detects good frames. Still work to do.

Carles Fernandez carles_fernandez-guest at moszumanska.debian.org
Wed Sep 2 00:22:38 UTC 2015


This is an automated email from the git hooks/post-receive script.

carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.

commit c23654f7a20c3b567b51697ad53c764942250e95
Author: Javier <jarribas at cttc.es>
Date:   Thu May 14 18:53:12 2015 +0200

    Updates in the GPS L2C CNAV telemetry decoder. Migrating code from SBAS
    decoder. Viterbi is working now and CRC check detects good frames. Still
    work to do.
---
 .../adapters/gps_l2_m_telemetry_decoder.cc         |   9 +-
 .../gps_l2_m_telemetry_decoder_cc.cc               | 688 ++++++++++++---------
 .../gps_l2_m_telemetry_decoder_cc.h                | 134 ++--
 .../gps_l2_m_dll_pll_tracking_cc.cc                |   5 +-
 4 files changed, 482 insertions(+), 354 deletions(-)

diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l2_m_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l2_m_telemetry_decoder.cc
index da02c29..94a79a1 100644
--- a/src/algorithms/telemetry_decoder/adapters/gps_l2_m_telemetry_decoder.cc
+++ b/src/algorithms/telemetry_decoder/adapters/gps_l2_m_telemetry_decoder.cc
@@ -38,6 +38,7 @@
 #include "gps_iono.h"
 #include "gps_utc_model.h"
 #include "configuration_interface.h"
+#include "concurrent_queue.h"
 
 extern concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
 extern concurrent_queue<Gps_Iono> global_gps_iono_queue;
@@ -70,10 +71,10 @@ GpsL2MTelemetryDecoder::GpsL2MTelemetryDecoder(ConfigurationInterface* configura
     telemetry_decoder_ = gps_l2_m_make_telemetry_decoder_cc(satellite_, 0, (long)fs_in, vector_length_, queue_, dump_); // TODO fix me
     DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
     // set the navigation msg queue;
-    telemetry_decoder_->set_ephemeris_queue(&global_gps_ephemeris_queue);
-    telemetry_decoder_->set_iono_queue(&global_gps_iono_queue);
-    telemetry_decoder_->set_almanac_queue(&global_gps_almanac_queue);
-    telemetry_decoder_->set_utc_model_queue(&global_gps_utc_model_queue);
+    //telemetry_decoder_->set_ephemeris_queue(&global_gps_ephemeris_queue);
+    //telemetry_decoder_->set_iono_queue(&global_gps_iono_queue);
+    //telemetry_decoder_->set_almanac_queue(&global_gps_almanac_queue);
+    //telemetry_decoder_->set_utc_model_queue(&global_gps_utc_model_queue);
 
     //decimation factor
     int decimation_factor = configuration->property(role + ".decimation_factor", 1);
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
index c91fd0a..6a4cc82 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
@@ -29,28 +29,25 @@
  * -------------------------------------------------------------------------
  */
 
-/*!
- * \todo Clean this code and move the telemetry definitions to GPS_L1_CA system definitions file
- */
-
-
-#include "gps_l2_m_telemetry_decoder_cc.h"
 #include <iostream>
 #include <sstream>
-#include <boost/lexical_cast.hpp>
 #include <gnuradio/io_signature.h>
 #include <glog/logging.h>
+#include <boost/lexical_cast.hpp>
 #include "control_message_factory.h"
 #include "gnss_synchro.h"
-
-#ifndef _rotl
-#define _rotl(X,N)  ((X << N) ^ (X >> (32-N)))  // Used in the parity check algorithm
-#endif
+#include "gps_l2_m_telemetry_decoder_cc.h"
 
 using google::LogMessage;
-/*!
- * \todo name and move the magic numbers to GPS_L1_CA.h
- */
+
+// logging levels
+#define EVENT 2 	// logs important events which don't occur every block
+#define FLOW 3  	// logs the function calls of block processing functions
+#define SAMP_SYNC 4 // about 1 log entry per sample -> high output
+#define LMORE 5 	//
+
+
+
 gps_l2_m_telemetry_decoder_cc_sptr
 gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
         int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump)
@@ -61,16 +58,6 @@ gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long
 
 
 
-void gps_l2_m_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
-{
-    for (unsigned i = 0; i < 3; i++)
-        {
-            ninput_items_required[i] = d_samples_per_bit * 8; //set the required sample history
-        }
-}
-
-
-
 gps_l2_m_telemetry_decoder_cc::gps_l2_m_telemetry_decoder_cc(
         Gnss_Satellite satellite,
         long if_freq,
@@ -79,332 +66,463 @@ gps_l2_m_telemetry_decoder_cc::gps_l2_m_telemetry_decoder_cc(
         int vector_length,
         boost::shared_ptr<gr::msg_queue> queue,
         bool dump) :
-        gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
-        gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
+                gr::block("gps_l2_m_telemetry_decoder_cc",
+                gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
+                gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
 {
     // initialize internal vars
-    d_queue = queue;
     d_dump = dump;
     d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
-    d_vector_length = vector_length;
-    d_samples_per_bit = ( GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS ) / GPS_CA_TELEMETRY_RATE_BITS_SECOND;
+    LOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite;
     d_fs_in = fs_in;
-    //d_preamble_duration_seconds = (1.0 / GPS_CA_TELEMETRY_RATE_BITS_SECOND) * GPS_CA_PREAMBLE_LENGTH_BITS;
-    //std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n";
-    // set the preamble
-    unsigned short int preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE;
-
-    memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GPS_CA_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int));
-
-    // preamble bits to sampled symbols
-    d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * GPS_CA_PREAMBLE_LENGTH_BITS * d_samples_per_bit);
-    int n = 0;
-    for (int i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++)
-        {
-            for (unsigned int j = 0; j < d_samples_per_bit; j++)
-                {
-                    if (d_preambles_bits[i] == 1)
-                        {
-                            d_preambles_symbols[n] = 1;
-                        }
-                    else
-                        {
-                            d_preambles_symbols[n] = -1;
-                        }
-                    n++;
-                }
-        }
-    d_sample_counter = 0;
-    //d_preamble_code_phase_seconds = 0;
-    d_stat = 0;
-    d_preamble_index = 0;
-    d_symbol_accumulator = 0;
-    d_symbol_accumulator_counter = 0;
-    d_frame_bit_index = 0;
-    d_preamble_time_seconds = 0;
-    d_flag_frame_sync = false;
-    d_GPS_frame_4bytes = 0;
-    d_prev_GPS_frame_4bytes = 0;
-    d_flag_parity = false;
-    d_TOW_at_Preamble = 0;
-    d_TOW_at_current_symbol = 0;
-    flag_TOW_set = false;
-    d_average_count = 0;
+    d_block_size = d_samples_per_symbol * d_symbols_per_bit * d_block_size_in_bits*2; // two CNAV frames
+    d_decimation_output_factor=0;
+    //set_output_multiple (1);
+    d_average_count=0;
+    d_flag_invert_buffer_symbols=false;
+    d_flag_invert_input_symbols=false;
+    d_channel=0;
     //set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
 }
 
 
+
 gps_l2_m_telemetry_decoder_cc::~gps_l2_m_telemetry_decoder_cc()
 {
-    delete d_preambles_symbols;
     d_dump_file.close();
 }
 
 
 
-bool gps_l2_m_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
+void gps_l2_m_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
 {
-    unsigned int d1, d2, d3, d4, d5, d6, d7, t, parity;
-    /* XOR as many bits in parallel as possible.  The magic constants pick
-       up bits which are to be XOR'ed together to implement the GPS parity
-       check algorithm described in IS-GPS-200E.  This avoids lengthy shift-
-       and-xor loops. */
-    d1 = gpsword & 0xFBFFBF00;
-    d2 = _rotl(gpsword,1) & 0x07FFBF01;
-    d3 = _rotl(gpsword,2) & 0xFC0F8100;
-    d4 = _rotl(gpsword,3) & 0xF81FFE02;
-    d5 = _rotl(gpsword,4) & 0xFC00000E;
-    d6 = _rotl(gpsword,5) & 0x07F00001;
-    d7 = _rotl(gpsword,6) & 0x00003000;
-    t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
-    // Now XOR the 5 6-bit fields together to produce the 6-bit final result.
-    parity = t ^ _rotl(t,6) ^ _rotl(t,12) ^ _rotl(t,18) ^ _rotl(t,24);
-    parity = parity & 0x3F;
-    if (parity == (gpsword & 0x3F)) return(true);
-    else return(false);
+    unsigned ninputs = ninput_items_required.size ();
+    for (unsigned i = 0; i < ninputs; i++)
+        ninput_items_required[i] = noutput_items;
+    //LOG(INFO) << "forecast(): " << "noutput_items=" << noutput_items << "\tninput_items_required ninput_items_required.size()=" << ninput_items_required.size();
 }
 
+void gps_l2_m_telemetry_decoder_cc::set_decimation(int decimation)
+{
+    d_decimation_output_factor = decimation;
+}
 
 int gps_l2_m_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
         gr_vector_const_void_star &input_items,	gr_vector_void_star &output_items)
 {
-    int corr_value = 0;
-    int preamble_diff = 0;
+	//LOG(INFO) << "general_work(): " << "noutput_items=" << noutput_items << "\toutput_items real size=" << output_items.size() <<  "\tninput_items size=" << ninput_items.size() << "\tinput_items real size=" << input_items.size() << "\tninput_items[0]=" << ninput_items[0];
+    // get pointers on in- and output gnss-synchro objects
+    const Gnss_Synchro *in = (const Gnss_Synchro *)  input_items[0]; // input
+    Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; 	// output
+
+    // store the time stamp of the first sample in the processed sample block
+    double sample_stamp = in[0].Tracking_timestamp_secs;
+
+    // copy correlation samples into samples vector
+    //for (int i = 0; i < noutput_items; i++)
+    ///    {
+            // check if channel is in tracking state
+    //       {
+                d_sample_buf.push_back(in[0].Prompt_I);
+    //        }
+    //    }
 
-    Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
-    d_sample_counter++; //count for the processed samples
+    consume_each(1); //one by one
 
-    // ########### Output the tracking data to navigation and PVT ##########
-    const Gnss_Synchro **in = (const Gnss_Synchro **)  &input_items[0]; //Get the input samples pointer
+    // decode only if enough samples in buffer
+    if(d_sample_buf.size() >= d_block_size)
+        {
+			d_flag_invert_buffer_symbols=false;
+    		while (true)
+    		{
+
+				if (d_flag_invert_buffer_symbols==true)
+				{
+					for (std::vector<double>::iterator symbol_it = d_sample_buf.begin(); symbol_it != d_sample_buf.end(); symbol_it++)
+					{
+						*symbol_it = -(*symbol_it);
+					}
+					LOG(INFO)<<"Inverting buffer symbols";
+				}
+
+				//debug
+				std::stringstream ss2;
+				for (std::vector<double>::const_iterator symbol_it = d_sample_buf.begin(); symbol_it < d_sample_buf.end(); ++symbol_it)
+					{
+						if(*symbol_it>=0)
+						{
+							ss2<<'1';
+						}else{
+							ss2<<'0';
+						}
+					}
+				LOG(INFO)<<"get_symbols="<<ss2.str();
+
+
+				// align symbols in pairs
+				// and obtain the bits by decoding the symbols (viterbi decoder)
+				// they can be already aligned or shifted by one position
+				std::vector<int> bits;
+				bool symbol_alignment = d_symbol_aligner_and_decoder.get_bits(d_sample_buf, bits);
+
+				std::stringstream ss;
+
+				for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
+					{
+						ss << *bit_it;
+					}
+
+				LOG(INFO)<<"get_bits="<<ss.str()<<std::endl;
+				// search for preambles
+				// and extract the corresponding message candidates
+				std::vector<msg_candiate_int_t> msg_candidates;
+				d_frame_detector.get_frame_candidates(bits, msg_candidates);
+
+				// verify checksum
+				// and return the valid messages
+				std::vector<msg_candiate_char_t> valid_msgs;
+				d_crc_verifier.get_valid_frames(msg_candidates, valid_msgs);
+				if (valid_msgs.size()==0)
+				{
+					if (d_flag_invert_buffer_symbols==false)
+					{
+						d_flag_invert_buffer_symbols=true;
+					}else{//already tested the symbol inversion but CRC still fail
+						LOG(INFO)<<"Discarting this buffer, no CNAV frames detected";
+						break;
+					}
+				}else{ //at least one frame has good CRC, keep the invert sign for the next frames
+					d_flag_invert_input_symbols=d_flag_invert_buffer_symbols;
+					LOG(INFO)<<valid_msgs.size()<<" GOOD L2C CNAV FRAME DETECTED!";
+					break;
+				}
+    		}
+//            // compute message sample stamp
+//            // and fill messages in SBAS raw message objects
+//            std::vector<Sbas_Raw_Msg> sbas_raw_msgs;
+//            for(std::vector<msg_candiate_char_t>::const_iterator it = valid_msgs.begin();
+//                    it != valid_msgs.end(); ++it)
+//                {
+//                    int message_sample_offset =
+//                            (sample_alignment ? 0 : -1)
+//                            + d_samples_per_symbol*(symbol_alignment ? -1 : 0)
+//                            + d_samples_per_symbol * d_symbols_per_bit * it->first;
+//                    double message_sample_stamp = sample_stamp + ((double)message_sample_offset)/1000;
+//                    VLOG(EVENT) << "message_sample_stamp=" << message_sample_stamp
+//                            << " (sample_stamp=" << sample_stamp
+//                            << " sample_alignment=" << sample_alignment
+//                            << " symbol_alignment=" << symbol_alignment
+//                            << " relative_preamble_start=" << it->first
+//                            << " message_sample_offset=" << message_sample_offset
+//                            << ")";
+//                    Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second);
+//                    sbas_raw_msgs.push_back(sbas_raw_msg);
+//                }
+//
+//            // parse messages
+//            // and send them to the SBAS raw message queue
+//            for(std::vector<Sbas_Raw_Msg>::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++)
+//                {
+//                    std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl;
+//                    sbas_telemetry_data.update(*it);
+//                }
+
+            // clear all processed samples in the input buffer
+            d_sample_buf.clear();
+        }
 
-    // TODO Optimize me!
-    //******* preamble correlation ********
-    for (unsigned int i = 0; i < d_samples_per_bit*8; i++)
+    // UPDATE GNSS SYNCHRO DATA
+    Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
+            //1. Copy the current tracking output
+            current_synchro_data = in[0];
+            //2. Add the telemetry decoder information
+            current_synchro_data.Flag_valid_word = false; // indicate to observable block that this synchro object isn't valid for pseudorange computation
+
+    d_average_count++;
+    if (d_average_count == d_decimation_output_factor)
         {
-            if (in[0][i].Prompt_I < 0)	// symbols clipping
-                {
-                    corr_value -= d_preambles_symbols[i];
+            d_average_count = 0;
+            //3. Make the output (copy the object contents to the GNURadio reserved memory)
+            out[0] = current_synchro_data;
+            //std::cout<<"GPS L2 TLM output on CH="<<this->d_channel << " SAMPLE STAMP="<<d_sample_counter/d_decimation_output_factor<<std::endl;
+            return 1;
+        }
+    else
+        {
+            return 0;
+        }
+}
+
+
+
+void gps_l2_m_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite)
+{
+    d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
+    LOG(INFO) << "GPS L2C CNAV telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite;
+}
+
+
+
+void gps_l2_m_telemetry_decoder_cc::set_channel(int channel)
+{
+    d_channel = channel;
+    LOG(INFO) << "GPS L2C CNAV channel set to " << channel;
+}
+
+
+// ### helper class for symbol alignment and viterbi decoding ###
+gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder()
+{
+    // convolutional code properties
+    d_KK = 7;
+    int nn = 2;
+    int g_encoder[nn];
+    g_encoder[0] = 121; //171o
+    g_encoder[1] = 91; //133o
+
+    d_vd1 = new Viterbi_Decoder(g_encoder, d_KK, nn);
+    d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn);
+    d_past_symbol = 0;
+}
+
+
+gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
+{
+    delete d_vd1;
+    delete d_vd2;
+}
+
+
+void gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
+{
+    d_past_symbol = 0;
+    d_vd1->reset();
+    d_vd2->reset();
+}
+
+
+bool gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector<double> symbols, std::vector<int> &bits)
+{
+    const int traceback_depth = 5*d_KK;
+    int nbits_requested = symbols.size()/d_symbols_per_bit;
+    int nbits_decoded;
+    // fill two vectors with the two possible symbol alignments
+    std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
+    std::vector<double> symbols_vd2;  // shifted symbol vector -> add past sample in front of input vector
+    symbols_vd2.push_back(d_past_symbol);
+    for (std::vector<double>::const_iterator symbol_it = symbols.begin(); symbol_it != symbols.end() - 1; ++symbol_it)
+        {
+            symbols_vd2.push_back(*symbol_it);
+        }
+    // arrays for decoded bits
+    int * bits_vd1 = new int[nbits_requested];
+    int * bits_vd2 = new int[nbits_requested];
+    // decode
+    float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded);
+    float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2, nbits_requested, nbits_decoded);
+    LOG(INFO)<<"metric_vd1="<<metric_vd1<<" metric_vd2="<<metric_vd2;
+    // choose the bits with the better metric
+    for (int i = 0; i < nbits_decoded; i++)
+        {
+            if (metric_vd1 > metric_vd2)
+                {// symbols aligned
+                    bits.push_back(bits_vd1[i]);
                 }
             else
-                {
-                    corr_value += d_preambles_symbols[i];
+                {// symbols shifted
+                    bits.push_back(bits_vd2[i]);
                 }
         }
-    d_flag_preamble = false;
+    d_past_symbol = symbols.back();
+    delete[] bits_vd1;
+    delete[] bits_vd2;
+    return metric_vd1 > metric_vd2;
+}
 
-    //******* frame sync ******************
-    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
-                    LOG(INFO) << "Preamble detection for SAT " << this->d_satellite;
-                    d_symbol_accumulator = 0; //sync the symbol to bits integrator
-                    d_symbol_accumulator_counter = 0;
-                    d_frame_bit_index = 8;
-                    d_stat = 1; // enter into frame pre-detection status
-                }
-            else if (d_stat == 1) //check 6 seconds of preamble separation
-                {
-                    preamble_diff = d_sample_counter - d_preamble_index;
-                    if (abs(preamble_diff - 6000) < 1)
-                        {
-                            d_GPS_FSM.Event_gps_word_preamble();
-                            d_flag_preamble = true;
-                            d_preamble_index = d_sample_counter;  //record the preamble sample stamp (t_P)
-                            d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;// - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
 
-                            if (!d_flag_frame_sync)
-                                {
-                                    d_flag_frame_sync = true;
-                                    LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
-                                }
-                        }
-                }
+// ### helper class for detecting the preamble and collect the corresponding message candidates ###
+void gps_l2_m_telemetry_decoder_cc::frame_detector::reset()
+{
+    d_buffer.clear();
+}
+
+
+void gps_l2_m_telemetry_decoder_cc::frame_detector::get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std::vector<int>>> &msg_candidates)
+{
+    std::stringstream ss;
+    unsigned int sbas_msg_length = 300;
+    std::vector<std::vector<int>> preambles = {{1, 0, 0, 0, 1, 0, 1 ,1}};
+    //LOG(INFO) << "get_frame_candidates(): " << "d_buffer.size()=" << d_buffer.size() << "\tbits.size()=" << bits.size();
+    ss << "copy bits ";
+    int count = 0;
+    // copy new bits into the working buffer
+    for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
+        {
+            d_buffer.push_back(*bit_it);
+            ss << *bit_it;
+            count++;
         }
-    else
+    //LOG(INFO) << ss.str() << " into working buffer (" << count << " bits)";
+    int relative_preamble_start = 0;
+    while(d_buffer.size() >= sbas_msg_length)
         {
-            if (d_stat == 1)
+            // compare with all preambles
+            for (std::vector<std::vector<int>>::iterator preample_it = preambles.begin(); preample_it < preambles.end(); ++preample_it)
                 {
-                    preamble_diff = d_sample_counter - d_preamble_index;
-                    if (preamble_diff > 6001)
+                    bool preamble_detected = true;
+                    bool inv_preamble_detected = true;
+                    // compare the buffer bits with the preamble bits
+                    for (std::vector<int>::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it)
                         {
-                            LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff;
-                            d_stat = 0; //lost of frame sync
-                            d_flag_frame_sync = false;
-                            flag_TOW_set = false;
+                            preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ;
+                            inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ;
+                        }
+                    if (preamble_detected || inv_preamble_detected)
+                        {
+                            // copy candidate
+                            std::vector<int> candidate;
+                            std::copy(d_buffer.begin(), d_buffer.begin() + sbas_msg_length, std::back_inserter(candidate));
+                            if(inv_preamble_detected)
+                                {
+                                    // invert bits
+                                    for (std::vector<int>::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++)
+                                        *candidate_bit_it = *candidate_bit_it == 0 ? 1 : 0;
+                                }
+                            msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate));
+                            ss.str("");
+                            ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate=";
+                            for (std::vector<int>::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
+                                ss << *bit_it;
+                            //LOG(INFO) << ss.str();
                         }
                 }
+            relative_preamble_start++;
+            // remove bit in front
+            d_buffer.pop_front();
         }
+}
+
+
+// ### helper class for checking the CRC of the message candidates ###
+
 
-    //******* SYMBOL TO BIT *******
-    d_symbol_accumulator += in[0][d_samples_per_bit*8 - 1].Prompt_I; // accumulate the input value in d_symbol_accumulator
-    d_symbol_accumulator_counter++;
-    if (d_symbol_accumulator_counter == 20)
+void gps_l2_m_telemetry_decoder_cc::crc_verifier::reset()
+{
+
+}
+
+void gps_l2_m_telemetry_decoder_cc::crc_verifier::get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs)
+{
+    std::stringstream ss;
+    LOG(INFO) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size();
+    // for each candidate
+    for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it)
         {
-            if (d_symbol_accumulator > 0)
-                { //symbol to bit
-                    d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
-                }
-            d_symbol_accumulator = 0;
-            d_symbol_accumulator_counter = 0;
-            //******* bits to words ******
-            d_frame_bit_index++;
-            if (d_frame_bit_index == 30)
+            // convert to bytes
+            std::vector<unsigned char> candidate_bytes;
+            zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes);
+            // verify CRC
+            d_checksum_agent.reset(0);
+            d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
+            unsigned int crc = d_checksum_agent.checksum();
+            LOG(INFO) << "candidate " << ": final crc remainder= " << std::hex << crc
+                            << std::setfill(' ') << std::resetiosflags(std::ios::hex);
+            //  the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
+            if (crc == 0)
                 {
-                    d_frame_bit_index = 0;
-                    // parity check
-                    // Each word in wordbuff is composed of:
-                    //      Bits 0 to 29 = the GPS data word
-                    //      Bits 30 to 31 = 2 LSBs of the GPS word ahead.
-                    // prepare the extended frame [-2 -1 0 ... 30]
-                    if (d_prev_GPS_frame_4bytes & 0x00000001)
-                        {
-                            d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000;
-                        }
-                    if (d_prev_GPS_frame_4bytes & 0x00000002)
-                        {
-                            d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000;
-                        }
-                    /* Check that the 2 most recently logged words pass parity. Have to first
-                     invert the data bits according to bit 30 of the previous word. */
-                    if(d_GPS_frame_4bytes & 0x40000000)
-                        {
-                            d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
-                        }
-                    if (gps_l2_m_telemetry_decoder_cc::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_time_seconds*1000.0;
-                            d_GPS_FSM.Event_gps_word_valid();
-                            d_flag_parity = true;
-                        }
-                    else
-                        {
-                            d_GPS_FSM.Event_gps_word_invalid();
-                            d_flag_parity = false;
-                        }
-                    d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
-                    d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
+                    valid_msgs.push_back(msg_candiate_char_t(candidate_it->first, candidate_bytes));
+                    ss << "Valid message found!";
                 }
             else
                 {
-                    d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
+                    ss << "Not a valid message.";
                 }
-        }
-    // output the frame
-    consume_each(1); //one by one
-    Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
-    //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)
-        //update TOW at the preamble instant (todo: check for valid d_TOW)
-        // 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.
-        // 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.
-        {
-            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_current_symbol = d_TOW_at_Preamble;//GPS_L1_CA_CODE_PERIOD;// + (double)GPS_CA_PREAMBLE_LENGTH_BITS/(double)GPS_CA_TELEMETRY_RATE_BITS_SECOND;
-            Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
-            if (flag_TOW_set == false)
+            //ss << " Relbitoffset=" << candidate_it->first << " content=";
+            for (std::vector<unsigned char>::iterator byte_it = candidate_bytes.begin(); byte_it < candidate_bytes.end(); ++byte_it)
                 {
-                    flag_TOW_set = true;
+                    ss << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)(*byte_it);
                 }
+            LOG(INFO) << ss.str() << std::setfill(' ') << std::resetiosflags(std::ios::hex) << std::endl;
         }
-    else
-        {
-            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;
 
-    current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; // to be  used in the hybrid configuration
-    current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true);
-    current_synchro_data.Flag_preamble = d_flag_preamble;
-    current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
-    current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
 
-    if(d_dump == true)
-        {
-            // MULTIPLEXED FILE RECORDING - Record results to file
-            try
-            {
-                    double tmp_double;
-                    tmp_double = d_TOW_at_current_symbol;
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    tmp_double = current_synchro_data.Prn_timestamp_ms;
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-                    tmp_double = d_TOW_at_Preamble;
-                    d_dump_file.write((char*)&tmp_double, sizeof(double));
-            }
-            catch (std::ifstream::failure e)
-            {
-                    LOG(WARNING) << "Exception writing observables dump file " << e.what();
-            }
-        }
 
-    //todo: implement averaging
 
-    d_average_count++;
-    if (d_average_count == d_decimation_output_factor)
-        {
-            d_average_count = 0;
-            //3. Make the output (copy the object contents to the GNURadio reserved memory)
-            *out[0] = current_synchro_data;
-            //std::cout<<"GPS L2 TLM output on CH="<<this->d_channel << " SAMPLE STAMP="<<d_sample_counter/d_decimation_output_factor<<std::endl;
-            return 1;
-        }
-    else
+void gps_l2_m_telemetry_decoder_cc::crc_verifier::zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
+{
+    std::stringstream ss;
+    const size_t bits_per_byte = 8;
+    unsigned char byte = 0;
+    //LOG(INFO) << "zerropad_back_and_convert_to_bytes():" << byte;
+    for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
         {
-            return 0;
+            int idx_bit = candidate_bit_it - msg_candidate.begin();
+            int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
+            byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
+            ss << *candidate_bit_it;
+            if (idx_bit % bits_per_byte == bits_per_byte - 1)
+                {
+                    bytes.push_back(byte);
+                    //LOG(INFO) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
+                    byte = 0;
+                }
         }
+    bytes.push_back(byte); // implies: insert 6 zeros at the end to fit the 250bits into a multiple of bytes
+    LOG(INFO) << " -> byte=" << std::setw(2)
+                << std::setfill('0') << std::hex << (unsigned int)byte
+                << std::setfill(' ') << std::resetiosflags(std::ios::hex);
 }
 
 
-void gps_l2_m_telemetry_decoder_cc::set_decimation(int decimation)
-{
-    d_decimation_output_factor = decimation;
-}
-
-void gps_l2_m_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite)
-{
-    d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
-    LOG(INFO) << "Setting decoder Finite State Machine to satellite "  << d_satellite;
-    d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN();
-    DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
-}
-
 
-void gps_l2_m_telemetry_decoder_cc::set_channel(int channel)
+void gps_l2_m_telemetry_decoder_cc::crc_verifier::zerropad_front_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
 {
-    d_channel = channel;
-    d_GPS_FSM.i_channel_ID = channel;
-    DLOG(INFO) << "Navigation channel set to " << channel;
-    // ############# ENABLE DATA FILE LOG #################
-    if (d_dump == true)
+    std::stringstream ss;
+    const size_t bits_per_byte = 8;
+    unsigned char byte = 0;
+    int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes
+    LOG(INFO) << "zerropad_front_and_convert_to_bytes():" << byte;
+    for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
         {
-            if (d_dump_file.is_open() == false)
+            int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
+            byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
+            ss << *candidate_bit_it;
+            if (idx_bit % bits_per_byte == bits_per_byte - 1)
                 {
-                    try
-                    {
-                            d_dump_filename = "telemetry";
-                            d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
-                            d_dump_filename.append(".dat");
-                            d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
-                            d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
-                            LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel
-                                      << " Log file: " << d_dump_filename.c_str();
-                    }
-                    catch (std::ifstream::failure e)
-                    {
-                            LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
-                    }
+                    bytes.push_back(byte);
+                    LOG(INFO) << ss.str() << " -> byte=" << std::setw(2)
+                                << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
+                    byte = 0;
                 }
+            idx_bit++;
         }
+    LOG(INFO) << " -> byte=" << std::setw(2)
+                << std::setfill('0') << std::hex << (unsigned int)byte
+                << std::setfill(' ') << std::resetiosflags(std::ios::hex);
 }
 
+//
+//void gps_l2_m_telemetry_decoder_cc::set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue)
+//{
+//    sbas_telemetry_data.set_raw_msg_queue(raw_msg_queue);
+//}
+//
+//
+//void gps_l2_m_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
+//{
+//    sbas_telemetry_data.set_iono_queue(iono_queue);
+//}
+//
+//
+//void gps_l2_m_telemetry_decoder_cc::set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue)
+//{
+//    sbas_telemetry_data.set_sat_corr_queue(sat_corr_queue);
+//}
+//
+//
+//void gps_l2_m_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
+//{
+//    sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);
+//}
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.h
index cbb5a68..14e8e8a 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.h
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.h
@@ -31,16 +31,18 @@
 #ifndef GNSS_SDR_GPS_L2_M_TELEMETRY_DECODER_CC_H
 #define	GNSS_SDR_GPS_L2_M_TELEMETRY_DECODER_CC_H
 
+#include <algorithm> // for copy
+#include <deque>
 #include <fstream>
 #include <string>
+#include <utility> // for pair
+#include <vector>
+#include <boost/crc.hpp>
 #include <gnuradio/block.h>
 #include <gnuradio/msg_queue.h>
-#include "GPS_L1_CA.h"
-#include "gps_l1_ca_subframe_fsm.h"
-#include "concurrent_queue.h"
 #include "gnss_satellite.h"
-
-
+#include "viterbi_decoder.h"
+//#include "sbas_telemetry_data.h"
 
 class gps_l2_m_telemetry_decoder_cc;
 
@@ -51,7 +53,7 @@ gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long
     int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
 
 /*!
- * \brief This class implements a block that decodes the NAV data defined in IS-GPS-200E
+ * \brief This class implements a block that decodes the SBAS integrity and corrections data defined in RTCA MOPS DO-229
  *
  */
 class gps_l2_m_telemetry_decoder_cc : public gr::block
@@ -61,20 +63,12 @@ public:
     void set_satellite(Gnss_Satellite satellite);  //!< Set satellite PRN
     void set_channel(int channel);                 //!< Set receiver's channel
 
-
-    /*!
-     * \brief Set decimation factor to average the GPS synchronization estimation output from the tracking module.
-     */
+    // queues to communicate broadcasted SBAS data to other blocks of GNSS-SDR
+    //void set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue);                 //!< Set raw msg queue
+    //void set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue);         //!< Set iono queue
+    //void set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue);  //!< Set sat correction queue
+    //void set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);           //!< Set SBAS ephemeis queue
     void set_decimation(int decimation);
-
-    /*!
-     * \brief Set the satellite data queue
-     */
-    void set_ephemeris_queue(concurrent_queue<Gps_Ephemeris> *ephemeris_queue){d_GPS_FSM.d_ephemeris_queue = ephemeris_queue;} //!< Set the ephemeris data queue
-    void set_iono_queue(concurrent_queue<Gps_Iono> *iono_queue){d_GPS_FSM.d_iono_queue = iono_queue;}                          //!< Set the iono data queue
-    void set_almanac_queue(concurrent_queue<Gps_Almanac> *almanac_queue){d_GPS_FSM.d_almanac_queue = almanac_queue;}           //!< Set the almanac data queue
-    void set_utc_model_queue(concurrent_queue<Gps_Utc_Model> *utc_model_queue){d_GPS_FSM.d_utc_model_queue = utc_model_queue;} //!< Set the UTC model data queue
-
     /*!
      * \brief This is where all signal processing takes place
      */
@@ -91,62 +85,78 @@ private:
     friend gps_l2_m_telemetry_decoder_cc_sptr
     gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in,unsigned
             int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
-
     gps_l2_m_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
             int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
 
-    bool gps_word_parityCheck(unsigned int gpsword);
-
-    // constants
-    unsigned short int d_preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS];
-    // class private vars
-
-    signed int *d_preambles_symbols;
-    unsigned int d_samples_per_bit;
-    long unsigned int d_sample_counter;
-    long unsigned int d_preamble_index;
-    unsigned int d_stat;
-    bool d_flag_frame_sync;
+    void viterbi_decoder(double *page_part_symbols, int *page_part_bits);
+    void align_samples();
 
-    // symbols
-    double d_symbol_accumulator;
-    short int d_symbol_accumulator_counter;
-
-    //bits and frame
-    unsigned short int d_frame_bit_index;
-    unsigned int d_GPS_frame_4bytes;
-    unsigned int d_prev_GPS_frame_4bytes;
-    bool d_flag_parity;
-    bool d_flag_preamble;
-    int d_word_number;
-
-    // output averaging and decimation
-    int d_average_count;
-    int d_decimation_output_factor;
+    static const int d_samples_per_symbol = 1;
+    static const int d_symbols_per_bit = 2;
+    static const int d_block_size_in_bits = 300;
 
     long d_fs_in;
-    //double d_preamble_duration_seconds;
-    // navigation message vars
-    Gps_Navigation_Message d_nav;
-    GpsL1CaSubframeFsm d_GPS_FSM;
 
-    boost::shared_ptr<gr::msg_queue> d_queue;
-    unsigned int d_vector_length;
     bool d_dump;
     Gnss_Satellite d_satellite;
     int d_channel;
 
-    //std::deque<double> d_prn_start_sample_history;
-
-    double d_preamble_time_seconds;
-
-    double d_TOW_at_Preamble;
-    double d_TOW_at_current_symbol;
-    double Prn_timestamp_at_preamble_ms;
-    bool flag_TOW_set;
-
     std::string d_dump_filename;
     std::ofstream d_dump_file;
+
+    bool d_flag_invert_input_symbols;
+    bool d_flag_invert_buffer_symbols;
+    int d_decimation_output_factor;
+    int d_average_count;
+    size_t d_block_size; //!< number of samples which are processed during one invocation of the algorithms
+    std::vector<double> d_sample_buf; //!< input buffer holding the samples to be processed in one block
+
+    typedef std::pair<int,std::vector<int>> msg_candiate_int_t;
+    typedef std::pair<int,std::vector<unsigned char>> msg_candiate_char_t;
+
+    // helper class for symbol alignment and Viterbi decoding
+    class symbol_aligner_and_decoder
+    {
+    public:
+        symbol_aligner_and_decoder();
+        ~symbol_aligner_and_decoder();
+        void reset();
+        bool get_bits(const std::vector<double> symbols, std::vector<int> &bits);
+    private:
+        int d_KK;
+        Viterbi_Decoder * d_vd1;
+        Viterbi_Decoder * d_vd2;
+        double d_past_symbol;
+    } d_symbol_aligner_and_decoder;
+
+
+    // helper class for detecting the preamble and collect the corresponding message candidates
+    class frame_detector
+    {
+    public:
+        void reset();
+        void get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int, std::vector<int>>> &msg_candidates);
+    private:
+        std::deque<int> d_buffer;
+    } d_frame_detector;
+
+
+    // helper class for checking the CRC of the message candidates
+    class crc_verifier
+    {
+    public:
+        void reset();
+        void get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs);
+    private:
+        typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
+        crc_24_q_type d_checksum_agent;
+        void zerropad_front_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes);
+        void zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes);
+    } d_crc_verifier;
+
+
+    //Sbas_Telemetry_Data sbas_telemetry_data;
 };
 
+
 #endif
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
index 3137325..b613369 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
@@ -212,7 +212,6 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
             corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
         }
     delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
-
     d_acq_code_phase_samples = corrected_acq_phase_samples;
 
     d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
@@ -605,7 +604,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
                     tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
                     d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
             }
-            catch (std::ifstream::failure e)
+            catch (std::ifstream::failure& e)
             {
                     LOG(WARNING) << "Exception writing trk dump file " << e.what();
             }
@@ -635,7 +634,7 @@ void gps_l2_m_dll_pll_tracking_cc::set_channel(unsigned int channel)
                             d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
                             LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
                     }
-                    catch (std::ifstream::failure e)
+                    catch (std::ifstream::failure& e)
                     {
                             LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
                     }

-- 
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git



More information about the pkg-hamradio-commits mailing list