2013-11-09 22:03:42 +00:00
/*!
* \ file sbas_l1_telemetry_decoder_cc . cc
* \ brief Implementation of a SBAS telemetry data decoder block
* \ author Daniel Fehr 2013. daniel . co ( at ) bluewin . ch
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2018-05-13 20:49:11 +00:00
* Copyright ( C ) 2010 - 2018 ( see AUTHORS file for a list of contributors )
2013-11-09 22:03:42 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
* GNSS - SDR is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
2015-01-08 18:49:59 +00:00
* ( at your option ) any later version .
2013-11-09 22:03:42 +00:00
*
* GNSS - SDR is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
2018-05-13 20:49:11 +00:00
* along with GNSS - SDR . If not , see < https : //www.gnu.org/licenses/>.
2013-11-09 22:03:42 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
2018-02-26 02:15:53 +00:00
# include "sbas_l1_telemetry_decoder_cc.h"
2013-11-09 22:03:42 +00:00
# include "control_message_factory.h"
# include "gnss_synchro.h"
2018-02-26 02:15:53 +00:00
# include <boost/lexical_cast.hpp>
# include <glog/logging.h>
# include <gnuradio/io_signature.h>
# include <sstream>
2013-11-09 22:03:42 +00:00
using google : : LogMessage ;
// logging levels
2018-03-03 01:03:39 +00:00
# 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 //
2013-11-09 22:03:42 +00:00
sbas_l1_telemetry_decoder_cc_sptr
2018-03-03 01:03:39 +00:00
sbas_l1_make_telemetry_decoder_cc ( const Gnss_Satellite & satellite , bool dump )
2013-11-09 22:03:42 +00:00
{
2016-04-13 15:18:29 +00:00
return sbas_l1_telemetry_decoder_cc_sptr ( new sbas_l1_telemetry_decoder_cc ( satellite , dump ) ) ;
2013-11-09 22:03:42 +00:00
}
2013-11-17 10:48:27 +00:00
2013-11-09 22:03:42 +00:00
sbas_l1_telemetry_decoder_cc : : sbas_l1_telemetry_decoder_cc (
2018-03-03 01:03:39 +00:00
const Gnss_Satellite & satellite ,
bool dump ) : gr : : block ( " sbas_l1_telemetry_decoder_cc " ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2013-11-09 22:03:42 +00:00
{
2016-04-12 15:29:16 +00:00
// Ephemeris data port out
this - > message_port_register_out ( pmt : : mp ( " telemetry " ) ) ;
2013-11-09 22:03:42 +00:00
// initialize internal vars
d_dump = dump ;
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " SBAS L1 TELEMETRY PROCESSING: satellite " < < d_satellite ;
2013-11-09 22:03:42 +00:00
d_block_size = d_samples_per_symbol * d_symbols_per_bit * d_block_size_in_bits ;
2015-05-22 16:54:03 +00:00
d_channel = 0 ;
2018-03-03 01:03:39 +00:00
set_output_multiple ( 1 ) ;
2013-11-09 22:03:42 +00:00
}
2013-11-17 10:48:27 +00:00
2013-11-09 22:03:42 +00:00
sbas_l1_telemetry_decoder_cc : : ~ sbas_l1_telemetry_decoder_cc ( )
{
2018-03-03 01:03:39 +00:00
if ( d_dump_file . is_open ( ) = = true )
2017-07-28 22:25:43 +00:00
{
try
2018-03-03 01:03:39 +00:00
{
2017-07-28 22:25:43 +00:00
d_dump_file . close ( ) ;
2018-03-03 01:03:39 +00:00
}
catch ( const std : : exception & ex )
{
2017-07-28 22:25:43 +00:00
LOG ( WARNING ) < < " Exception in destructor closing the dump file " < < ex . what ( ) ;
2018-03-03 01:03:39 +00:00
}
2017-07-28 22:25:43 +00:00
}
2013-11-09 22:03:42 +00:00
}
2013-11-17 10:48:27 +00:00
2018-03-03 01:03:39 +00:00
void sbas_l1_telemetry_decoder_cc : : set_satellite ( const Gnss_Satellite & satellite )
2013-11-09 22:03:42 +00:00
{
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " SBAS telemetry decoder in channel " < < this - > d_channel < < " set to satellite " < < d_satellite ;
2013-11-09 22:03:42 +00:00
}
2018-08-13 11:03:01 +00:00
void sbas_l1_telemetry_decoder_cc : : set_channel ( int32_t channel )
2013-11-09 22:03:42 +00:00
{
d_channel = channel ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " SBAS channel set to " < < channel ;
2013-11-09 22:03:42 +00:00
}
// ### helper class for sample alignment ###
sbas_l1_telemetry_decoder_cc : : sample_aligner : : sample_aligner ( )
{
d_n_smpls_in_history = 3 ;
d_iir_par = 0.05 ;
reset ( ) ;
}
2013-11-10 18:52:29 +00:00
2018-12-03 10:12:10 +00:00
sbas_l1_telemetry_decoder_cc : : sample_aligner : : ~ sample_aligner ( ) = default ;
2013-11-09 22:03:42 +00:00
void sbas_l1_telemetry_decoder_cc : : sample_aligner : : reset ( )
{
d_past_sample = 0 ;
d_corr_paired = 0 ;
d_corr_shifted = 0 ;
d_aligned = true ;
}
2013-11-10 18:52:29 +00:00
2013-11-09 22:03:42 +00:00
/*
* samples length must be a multiple of two
*/
bool sbas_l1_telemetry_decoder_cc : : sample_aligner : : get_symbols ( const std : : vector < double > samples , std : : vector < double > & symbols )
{
2018-03-03 01:03:39 +00:00
double smpls [ 3 ] = { } ;
2013-11-09 22:03:42 +00:00
double corr_diff ;
bool stand_by = true ;
double sym ;
2018-03-03 01:03:39 +00:00
VLOG ( FLOW ) < < " get_symbols(): "
< < " d_past_sample= " < < d_past_sample < < " \t samples size= " < < samples . size ( ) ;
2013-11-09 22:03:42 +00:00
2018-08-13 11:03:01 +00:00
for ( uint32_t i_sym = 0 ; i_sym < samples . size ( ) / sbas_l1_telemetry_decoder_cc : : d_samples_per_symbol ; i_sym + + )
2013-11-09 22:03:42 +00:00
{
// get the next samples
2018-08-13 11:03:01 +00:00
for ( int32_t i = 0 ; i < d_n_smpls_in_history ; i + + )
2013-11-09 22:03:42 +00:00
{
2018-08-13 11:03:01 +00:00
smpls [ i ] = static_cast < int32_t > ( i_sym ) * sbas_l1_telemetry_decoder_cc : : d_samples_per_symbol + i - 1 = = - 1 ? d_past_sample : samples [ i_sym * sbas_l1_telemetry_decoder_cc : : d_samples_per_symbol + i - 1 ] ;
2013-11-09 22:03:42 +00:00
}
// update the pseudo correlations (IIR method) of the two possible alignments
2018-03-03 01:03:39 +00:00
d_corr_paired = d_iir_par * smpls [ 1 ] * smpls [ 2 ] + ( 1 - d_iir_par ) * d_corr_paired ;
d_corr_shifted = d_iir_par * smpls [ 0 ] * smpls [ 1 ] + ( 1 - d_iir_par ) * d_corr_shifted ;
2013-11-09 22:03:42 +00:00
// decide which alignment is the correct one
2013-11-10 18:52:29 +00:00
corr_diff = std : : abs ( d_corr_paired - d_corr_shifted ) ;
2018-03-03 01:03:39 +00:00
stand_by = d_aligned ? corr_diff < d_corr_paired / 2 : corr_diff < d_corr_shifted / 2 ;
2013-11-09 22:03:42 +00:00
if ( ! stand_by )
{
d_aligned = d_corr_paired > = d_corr_shifted ;
}
// sum the correct pair of samples to a symbol, depending on the current alignment d_align
2018-08-13 11:03:01 +00:00
sym = smpls [ 0 + int32_t ( d_aligned ) * 2 ] + smpls [ 1 ] ;
2013-11-09 22:03:42 +00:00
symbols . push_back ( sym ) ;
// sample alignment debug output
VLOG ( SAMP_SYNC ) < < std : : setprecision ( 5 )
2018-03-03 01:03:39 +00:00
< < " smplp: " < < std : : setw ( 6 ) < < smpls [ 0 ] < < " "
< < " smpl0: " < < std : : setw ( 6 )
< < smpls [ 1 ] < < " "
< < " smpl1: " < < std : : setw ( 6 ) < < smpls [ 2 ] < < " \t "
//<< "Flag_valid_tracking: " << std::setw(1) << in[0][0].Flag_valid_tracking << " " << std::setw(1) << in[0][0].Flag_valid_tracking << "\t"
< < " d_corr_paired: " < < std : : setw ( 10 ) < < d_corr_paired < < " \t "
< < " d_corr_shifted: " < < std : : setw ( 10 ) < < d_corr_shifted < < " \t "
< < " corr_diff: " < < std : : setw ( 10 ) < < corr_diff < < " \t "
< < " stand_by: " < < std : : setw ( 1 ) < < stand_by < < " \t "
< < " d_aligned: " < < std : : setw ( 1 ) < < d_aligned < < " \t "
< < " sym: " < < std : : setw ( 10 ) < < sym < < " \t " ;
2013-11-09 22:03:42 +00:00
}
// save last sample for next block
2018-03-03 01:03:39 +00:00
double temp ;
2013-11-09 22:03:42 +00:00
temp = samples . back ( ) ;
d_past_sample = ( temp ) ;
return d_aligned ;
}
// ### helper class for symbol alignment and viterbi decoding ###
sbas_l1_telemetry_decoder_cc : : symbol_aligner_and_decoder : : symbol_aligner_and_decoder ( )
{
// convolutional code properties
d_KK = 7 ;
2018-08-16 23:29:29 +00:00
const int32_t nn = 2 ;
2018-08-13 11:03:01 +00:00
int32_t g_encoder [ nn ] ;
2013-11-09 22:03:42 +00:00
g_encoder [ 0 ] = 121 ;
g_encoder [ 1 ] = 91 ;
d_vd1 = new Viterbi_Decoder ( g_encoder , d_KK , nn ) ;
d_vd2 = new Viterbi_Decoder ( g_encoder , d_KK , nn ) ;
d_past_symbol = 0 ;
}
2013-11-10 18:52:29 +00:00
2013-11-09 22:03:42 +00:00
sbas_l1_telemetry_decoder_cc : : symbol_aligner_and_decoder : : ~ symbol_aligner_and_decoder ( )
{
delete d_vd1 ;
delete d_vd2 ;
}
2013-11-17 10:48:27 +00:00
2013-11-09 22:03:42 +00:00
void sbas_l1_telemetry_decoder_cc : : symbol_aligner_and_decoder : : reset ( )
{
d_past_symbol = 0 ;
d_vd1 - > reset ( ) ;
d_vd2 - > reset ( ) ;
}
2013-11-17 10:48:27 +00:00
2018-08-13 11:03:01 +00:00
bool sbas_l1_telemetry_decoder_cc : : symbol_aligner_and_decoder : : get_bits ( const std : : vector < double > symbols , std : : vector < int32_t > & bits )
2013-11-09 22:03:42 +00:00
{
2018-08-13 11:03:01 +00:00
const int32_t traceback_depth = 5 * d_KK ;
int32_t nbits_requested = symbols . size ( ) / d_symbols_per_bit ;
int32_t nbits_decoded ;
2013-11-09 22:03:42 +00:00
// fill two vectors with the two possible symbol alignments
2018-03-03 01:03:39 +00:00
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
2015-05-14 16:02:57 +00:00
symbols_vd2 . push_back ( d_past_symbol ) ;
2018-12-03 15:25:11 +00:00
for ( auto symbol_it = symbols . cbegin ( ) ; symbol_it ! = symbols . cend ( ) - 1 ; + + symbol_it )
2013-11-09 22:03:42 +00:00
{
symbols_vd2 . push_back ( * symbol_it ) ;
}
// arrays for decoded bits
2018-12-03 15:25:11 +00:00
auto * bits_vd1 = new int32_t [ nbits_requested ] ;
auto * bits_vd2 = new int32_t [ nbits_requested ] ;
2013-11-09 22:03:42 +00:00
// 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 ) ;
// choose the bits with the better metric
2018-08-13 11:03:01 +00:00
for ( int32_t i = 0 ; i < nbits_decoded ; i + + )
2013-11-09 22:03:42 +00:00
{
if ( metric_vd1 > metric_vd2 )
2018-03-03 01:03:39 +00:00
{ // symbols aligned
2013-11-09 22:03:42 +00:00
bits . push_back ( bits_vd1 [ i ] ) ;
}
else
2018-03-03 01:03:39 +00:00
{ // symbols shifted
2013-11-09 22:03:42 +00:00
bits . push_back ( bits_vd2 [ i ] ) ;
}
}
d_past_symbol = symbols . back ( ) ;
delete [ ] bits_vd1 ;
delete [ ] bits_vd2 ;
return metric_vd1 > metric_vd2 ;
}
// ### helper class for detecting the preamble and collect the corresponding message candidates ###
void sbas_l1_telemetry_decoder_cc : : frame_detector : : reset ( )
{
d_buffer . clear ( ) ;
}
2013-11-10 18:52:29 +00:00
2018-08-13 11:03:01 +00:00
void sbas_l1_telemetry_decoder_cc : : frame_detector : : get_frame_candidates ( const std : : vector < int32_t > bits , std : : vector < std : : pair < int32_t , std : : vector < int32_t > > > & msg_candidates )
2013-11-09 22:03:42 +00:00
{
std : : stringstream ss ;
2018-08-13 11:03:01 +00:00
uint32_t sbas_msg_length = 250 ;
std : : vector < std : : vector < int32_t > > preambles = { { 0 , 1 , 0 , 1 , 0 , 0 , 1 , 1 } ,
2018-03-03 01:03:39 +00:00
{ 1 , 0 , 0 , 1 , 1 , 0 , 1 , 0 } ,
{ 1 , 1 , 0 , 0 , 0 , 1 , 1 , 0 } } ;
VLOG ( FLOW ) < < " get_frame_candidates(): "
< < " d_buffer.size()= " < < d_buffer . size ( ) < < " \t bits.size()= " < < bits . size ( ) ;
2013-11-09 22:03:42 +00:00
ss < < " copy bits " ;
2018-08-13 11:03:01 +00:00
int32_t count = 0 ;
2013-11-09 22:03:42 +00:00
// copy new bits into the working buffer
2018-12-03 15:25:11 +00:00
for ( auto bit_it = bits . cbegin ( ) ; bit_it < bits . cend ( ) ; + + bit_it )
2013-11-09 22:03:42 +00:00
{
d_buffer . push_back ( * bit_it ) ;
ss < < * bit_it ;
count + + ;
}
VLOG ( SAMP_SYNC ) < < ss . str ( ) < < " into working buffer ( " < < count < < " bits) " ;
2018-08-13 11:03:01 +00:00
int32_t relative_preamble_start = 0 ;
2018-03-03 01:03:39 +00:00
while ( d_buffer . size ( ) > = sbas_msg_length )
2013-11-09 22:03:42 +00:00
{
// compare with all preambles
2018-12-03 15:25:11 +00:00
for ( auto preample_it = preambles . begin ( ) ; preample_it < preambles . end ( ) ; + + preample_it )
2013-11-09 22:03:42 +00:00
{
bool preamble_detected = true ;
bool inv_preamble_detected = true ;
// compare the buffer bits with the preamble bits
2018-12-03 15:25:11 +00:00
for ( auto preample_bit_it = preample_it - > begin ( ) ; preample_bit_it < preample_it - > end ( ) ; + + preample_bit_it )
2013-11-09 22:03:42 +00:00
{
2018-03-03 01:03:39 +00:00
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 ;
2013-11-09 22:03:42 +00:00
}
if ( preamble_detected | | inv_preamble_detected )
{
// copy candidate
2018-08-13 11:03:01 +00:00
std : : vector < int32_t > candidate ;
2013-11-10 18:52:29 +00:00
std : : copy ( d_buffer . begin ( ) , d_buffer . begin ( ) + sbas_msg_length , std : : back_inserter ( candidate ) ) ;
2018-03-03 01:03:39 +00:00
if ( inv_preamble_detected )
2013-11-09 22:03:42 +00:00
{
// invert bits
2018-12-03 17:03:25 +00:00
for ( int & candidate_bit_it : candidate )
candidate_bit_it = candidate_bit_it = = 0 ? 1 : 0 ;
2013-11-09 22:03:42 +00:00
}
2018-08-13 11:03:01 +00:00
msg_candidates . push_back ( std : : pair < int32_t , std : : vector < int32_t > > ( relative_preamble_start , candidate ) ) ;
2013-11-09 22:03:42 +00:00
ss . str ( " " ) ;
2018-03-03 01:03:39 +00:00
ss < < " preamble " < < preample_it - preambles . begin ( ) < < ( inv_preamble_detected ? " inverted " : " normal " ) < < " detected! candidate= " ;
2018-12-03 15:25:11 +00:00
for ( auto bit_it = candidate . begin ( ) ; bit_it < candidate . end ( ) ; + + bit_it )
2013-11-09 22:03:42 +00:00
ss < < * bit_it ;
VLOG ( EVENT ) < < ss . str ( ) ;
}
}
relative_preamble_start + + ;
// remove bit in front
d_buffer . pop_front ( ) ;
}
}
// ### helper class for checking the CRC of the message candidates ###
void sbas_l1_telemetry_decoder_cc : : crc_verifier : : reset ( )
{
}
2018-04-13 09:07:58 +00:00
2013-11-10 18:52:29 +00:00
void sbas_l1_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 )
2013-11-09 22:03:42 +00:00
{
std : : stringstream ss ;
2018-03-03 01:03:39 +00:00
VLOG ( FLOW ) < < " get_valid_frames(): "
< < " msg_candidates.size()= " < < msg_candidates . size ( ) ;
2013-11-09 22:03:42 +00:00
// for each candidate
2018-12-03 15:25:11 +00:00
for ( auto candidate_it = msg_candidates . cbegin ( ) ; candidate_it < msg_candidates . cend ( ) ; + + candidate_it )
2013-11-09 22:03:42 +00:00
{
// convert to bytes
2018-08-13 11:03:01 +00:00
std : : vector < uint8_t > candidate_bytes ;
2013-11-09 22:03:42 +00:00
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 ( ) ) ;
2018-08-13 11:03:01 +00:00
uint32_t crc = d_checksum_agent . checksum ( ) ;
2013-11-17 10:48:27 +00:00
VLOG ( SAMP_SYNC ) < < " candidate " < < candidate_it - msg_candidates . begin ( )
< < " : final crc remainder= " < < std : : hex < < crc
< < std : : setfill ( ' ' ) < < std : : resetiosflags ( std : : ios : : hex ) ;
2013-11-09 22:03:42 +00:00
// the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
if ( crc = = 0 )
{
2013-11-17 10:48:27 +00:00
valid_msgs . push_back ( msg_candiate_char_t ( candidate_it - > first , candidate_bytes ) ) ;
2013-11-09 22:03:42 +00:00
ss < < " Valid message found! " ;
}
else
{
ss < < " Not a valid message. " ;
}
ss < < " Relbitoffset= " < < candidate_it - > first < < " content= " ;
2018-12-03 15:25:11 +00:00
for ( auto byte_it = candidate_bytes . begin ( ) ; byte_it < candidate_bytes . end ( ) ; + + byte_it )
2013-11-09 22:03:42 +00:00
{
2018-08-13 11:03:01 +00:00
ss < < std : : setw ( 2 ) < < std : : setfill ( ' 0 ' ) < < std : : hex < < static_cast < uint32_t > ( ( * byte_it ) ) ;
2013-11-09 22:03:42 +00:00
}
VLOG ( SAMP_SYNC ) < < ss . str ( ) < < std : : setfill ( ' ' ) < < std : : resetiosflags ( std : : ios : : hex ) < < std : : endl ;
}
}
2013-11-10 18:52:29 +00:00
2018-08-13 11:03:01 +00:00
void sbas_l1_telemetry_decoder_cc : : crc_verifier : : zerropad_back_and_convert_to_bytes ( const std : : vector < int > msg_candidate , std : : vector < uint8_t > & bytes )
2013-11-09 22:03:42 +00:00
{
std : : stringstream ss ;
const size_t bits_per_byte = 8 ;
2018-08-13 11:03:01 +00:00
uint8_t byte = 0 ;
2013-11-09 22:03:42 +00:00
VLOG ( LMORE ) < < " zerropad_back_and_convert_to_bytes(): " < < byte ;
2018-12-03 15:25:11 +00:00
for ( auto candidate_bit_it = msg_candidate . cbegin ( ) ; candidate_bit_it < msg_candidate . cend ( ) ; + + candidate_bit_it )
2013-11-09 22:03:42 +00:00
{
2018-08-13 11:03:01 +00:00
int32_t idx_bit = candidate_bit_it - msg_candidate . begin ( ) ;
int32_t bit_pos_in_current_byte = ( bits_per_byte - 1 ) - ( idx_bit % bits_per_byte ) ;
byte | = static_cast < uint8_t > ( * candidate_bit_it ) < < bit_pos_in_current_byte ;
2013-11-09 22:03:42 +00:00
ss < < * candidate_bit_it ;
2013-11-10 18:52:29 +00:00
if ( idx_bit % bits_per_byte = = bits_per_byte - 1 )
2013-11-09 22:03:42 +00:00
{
bytes . push_back ( byte ) ;
2018-08-13 11:03:01 +00:00
VLOG ( LMORE ) < < ss . str ( ) < < " -> byte= " < < std : : setw ( 2 ) < < std : : setfill ( ' 0 ' ) < < std : : hex < < static_cast < uint32_t > ( byte ) ;
2018-03-03 01:03:39 +00:00
ss . str ( " " ) ;
2013-11-09 22:03:42 +00:00
byte = 0 ;
}
}
2018-03-03 01:03:39 +00:00
bytes . push_back ( byte ) ; // implies: insert 6 zeros at the end to fit the 250bits into a multiple of bytes
2013-11-17 10:48:27 +00:00
VLOG ( LMORE ) < < " -> byte= " < < std : : setw ( 2 )
2018-08-13 11:03:01 +00:00
< < std : : setfill ( ' 0 ' ) < < std : : hex < < static_cast < uint32_t > ( byte )
2013-11-17 10:48:27 +00:00
< < std : : setfill ( ' ' ) < < std : : resetiosflags ( std : : ios : : hex ) ;
2013-11-09 22:03:42 +00:00
}
2013-11-10 18:52:29 +00:00
2018-08-13 11:03:01 +00:00
void sbas_l1_telemetry_decoder_cc : : crc_verifier : : zerropad_front_and_convert_to_bytes ( const std : : vector < int32_t > msg_candidate , std : : vector < uint8_t > & bytes )
2013-11-09 22:03:42 +00:00
{
std : : stringstream ss ;
const size_t bits_per_byte = 8 ;
2018-08-13 11:03:01 +00:00
uint8_t byte = 0 ;
int32_t idx_bit = 6 ; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes
2013-11-09 22:03:42 +00:00
VLOG ( LMORE ) < < " zerropad_front_and_convert_to_bytes(): " < < byte ;
2018-12-03 15:25:11 +00:00
for ( auto candidate_bit_it = msg_candidate . cbegin ( ) ; candidate_bit_it < msg_candidate . cend ( ) ; + + candidate_bit_it )
2013-11-09 22:03:42 +00:00
{
2018-08-13 11:03:01 +00:00
int32_t bit_pos_in_current_byte = ( bits_per_byte - 1 ) - ( idx_bit % bits_per_byte ) ;
byte | = static_cast < uint8_t > ( * candidate_bit_it ) < < bit_pos_in_current_byte ;
2013-11-09 22:03:42 +00:00
ss < < * candidate_bit_it ;
2013-11-10 18:52:29 +00:00
if ( idx_bit % bits_per_byte = = bits_per_byte - 1 )
2013-11-09 22:03:42 +00:00
{
bytes . push_back ( byte ) ;
2013-11-17 10:48:27 +00:00
VLOG ( LMORE ) < < ss . str ( ) < < " -> byte= " < < std : : setw ( 2 )
2018-08-13 11:03:01 +00:00
< < std : : setfill ( ' 0 ' ) < < std : : hex < < static_cast < uint32_t > ( byte ) ;
2018-03-03 01:03:39 +00:00
ss . str ( " " ) ;
2013-11-09 22:03:42 +00:00
byte = 0 ;
}
idx_bit + + ;
}
2013-11-17 10:48:27 +00:00
VLOG ( LMORE ) < < " -> byte= " < < std : : setw ( 2 )
2018-08-13 11:03:01 +00:00
< < std : : setfill ( ' 0 ' ) < < std : : hex < < static_cast < uint32_t > ( byte )
2013-11-17 10:48:27 +00:00
< < std : : setfill ( ' ' ) < < std : : resetiosflags ( std : : ios : : hex ) ;
2013-11-09 22:03:42 +00:00
}
2018-04-13 09:07:58 +00:00
int sbas_l1_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 )
{
VLOG ( FLOW ) < < " general_work(): "
< < " noutput_items= " < < noutput_items < < " \t output_items real size= " < < output_items . size ( ) < < " \t ninput_items size= " < < ninput_items . size ( ) < < " \t input_items real size= " < < input_items . size ( ) < < " \t ninput_items[0]= " < < ninput_items [ 0 ] ;
// get pointers on in- and output gnss-synchro objects
2018-12-03 15:25:11 +00:00
auto * out = reinterpret_cast < Gnss_Synchro * > ( output_items [ 0 ] ) ; // Get the output buffer pointer
const auto * in = reinterpret_cast < const Gnss_Synchro * > ( input_items [ 0 ] ) ; // Get the input buffer pointer
2018-04-13 09:07:58 +00:00
2018-08-13 11:03:01 +00:00
Gnss_Synchro current_symbol ; // structure to save the synchronization information and send the output object to the next block
// 1. Copy the current tracking output
2018-04-13 09:07:58 +00:00
current_symbol = in [ 0 ] ;
// copy correlation samples into samples vector
d_sample_buf . push_back ( current_symbol . Prompt_I ) ; //add new symbol to the symbol queue
// store the time stamp of the first sample in the processed sample block
double sample_stamp = static_cast < double > ( in [ 0 ] . Tracking_sample_counter ) / static_cast < double > ( in [ 0 ] . fs ) ;
// decode only if enough samples in buffer
if ( d_sample_buf . size ( ) > = d_block_size )
{
// align correlation samples in pairs
// and obtain the symbols by summing the paired correlation samples
std : : vector < double > symbols ;
bool sample_alignment = d_sample_aligner . get_symbols ( d_sample_buf , symbols ) ;
// align symbols in pairs
// and obtain the bits by decoding the symbol pairs
2018-08-13 11:03:01 +00:00
std : : vector < int32_t > bits ;
2018-04-13 09:07:58 +00:00
bool symbol_alignment = d_symbol_aligner_and_decoder . get_bits ( symbols , bits ) ;
// 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 ) ;
// compute message sample stamp
// and fill messages in SBAS raw message objects
//std::vector<Sbas_Raw_Msg> sbas_raw_msgs;
2018-12-03 17:03:25 +00:00
for ( const auto & valid_msg : valid_msgs )
2018-04-13 09:07:58 +00:00
{
2018-08-13 11:03:01 +00:00
int32_t message_sample_offset =
2018-12-03 17:03:25 +00:00
( sample_alignment ? 0 : - 1 ) + d_samples_per_symbol * ( symbol_alignment ? - 1 : 0 ) + d_samples_per_symbol * d_symbols_per_bit * valid_msg . first ;
2018-04-13 09:07:58 +00:00
double message_sample_stamp = sample_stamp + static_cast < double > ( message_sample_offset ) / 1000.0 ;
VLOG ( EVENT ) < < " message_sample_stamp= " < < message_sample_stamp
< < " (sample_stamp= " < < sample_stamp
< < " sample_alignment= " < < sample_alignment
< < " symbol_alignment= " < < symbol_alignment
2018-12-03 17:03:25 +00:00
< < " relative_preamble_start= " < < valid_msg . first
2018-04-13 09:07:58 +00:00
< < " 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 ( ) ;
}
// UPDATE GNSS SYNCHRO DATA
// actually the SBAS telemetry decoder doesn't support ranging
current_symbol . Flag_valid_word = false ; // indicate to observable block that this synchro object isn't valid for pseudorange computation
out [ 0 ] = current_symbol ;
consume_each ( 1 ) ; // tell scheduler input items consumed
return 1 ; // tell scheduler output items produced
}