2018-07-06 12:42:13 +00:00
/*!
2019-03-02 01:21:03 +00:00
* \ file beidou_b1i_telemetry_decoder_gs . cc
2019-03-05 21:17:09 +00:00
* \ brief Implementation of a BEIDOU BI1 DNAV data decoder block
2018-12-19 16:20:12 +00:00
* \ note Code added as part of GSoC 2018 program
* \ author Damian Miralles , 2018. dmiralles2009 ( at ) gmail . com
2018-08-06 11:52:08 +00:00
* \ author Sergi Segura , 2018. sergi . segura . munoz ( at ) gmail . es
2018-07-06 12:42:13 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2019-02-10 11:40:03 +00:00
* Copyright ( C ) 2010 - 2019 ( see AUTHORS file for a list of contributors )
2018-07-06 12:42:13 +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
* ( at your option ) any later version .
*
* GNSS - SDR is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
2018-12-19 16:20:12 +00:00
* along with GNSS - SDR . If not , see < http : //www.gnu.org/licenses/>.
2018-07-06 12:42:13 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
2019-03-02 01:21:03 +00:00
# include "beidou_b1i_telemetry_decoder_gs.h"
2019-03-05 21:17:09 +00:00
# include "Beidou_B1I.h"
2019-07-10 15:49:41 +00:00
# include "Beidou_DNAV.h"
2019-07-10 17:52:40 +00:00
# include "beidou_dnav_almanac.h"
2019-03-05 21:17:09 +00:00
# include "beidou_dnav_ephemeris.h"
2019-03-06 08:35:20 +00:00
# include "beidou_dnav_iono.h"
2019-03-05 21:17:09 +00:00
# include "beidou_dnav_utc_model.h"
2019-07-04 15:19:33 +00:00
# include "display.h"
2018-12-19 16:20:12 +00:00
# include "gnss_synchro.h"
# include <glog/logging.h>
2019-01-28 01:29:43 +00:00
# include <gnuradio/io_signature.h>
2019-03-05 21:17:09 +00:00
# include <pmt/pmt.h> // for make_any
# include <pmt/pmt_sugar.h> // for mp
2019-07-19 16:23:36 +00:00
# include <cstdlib> // for abs
# include <exception> // for exception
# include <iostream> // for cout
# include <memory> // for shared_ptr, make_shared
2018-12-19 16:20:12 +00:00
# define CRC_ERROR_LIMIT 8
2018-07-06 12:42:13 +00:00
2019-03-02 01:21:03 +00:00
beidou_b1i_telemetry_decoder_gs_sptr
beidou_b1i_make_telemetry_decoder_gs ( const Gnss_Satellite & satellite , bool dump )
2018-07-06 12:42:13 +00:00
{
2019-03-02 01:21:03 +00:00
return beidou_b1i_telemetry_decoder_gs_sptr ( new beidou_b1i_telemetry_decoder_gs ( satellite , dump ) ) ;
2018-07-06 12:42:13 +00:00
}
2019-03-02 01:21:03 +00:00
beidou_b1i_telemetry_decoder_gs : : beidou_b1i_telemetry_decoder_gs (
2018-07-06 12:42:13 +00:00
const Gnss_Satellite & satellite ,
2019-03-02 01:21:03 +00:00
bool dump ) : gr : : block ( " beidou_b1i_telemetry_decoder_gs " ,
2019-01-28 01:29:43 +00:00
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2018-07-06 12:42:13 +00:00
{
2019-07-19 16:23:36 +00:00
// prevent telemetry symbols accumulation in output buffers
2019-07-04 15:19:33 +00:00
this - > set_max_noutput_items ( 1 ) ;
2018-07-06 12:42:13 +00:00
// Ephemeris data port out
this - > message_port_register_out ( pmt : : mp ( " telemetry " ) ) ;
2019-03-18 15:28:49 +00:00
// Control messages to tracking block
this - > message_port_register_out ( pmt : : mp ( " telemetry_to_trk " ) ) ;
2018-07-06 12:42:13 +00:00
// initialize internal vars
d_dump = dump ;
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
2019-03-07 15:38:49 +00:00
LOG ( INFO ) < < " Initializing BeiDou B1I Telemetry Decoding for satellite " < < this - > d_satellite ;
2018-12-26 18:54:23 +00:00
2019-07-10 17:52:40 +00:00
d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS ;
2019-01-28 01:29:43 +00:00
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
2019-07-10 17:52:40 +00:00
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ;
2019-01-28 01:29:43 +00:00
// Setting samples of preamble code
for ( int32_t i = 0 ; i < d_symbols_per_preamble ; i + + )
{
if ( BEIDOU_DNAV_PREAMBLE . at ( i ) = = ' 1 ' )
{
2019-07-10 17:52:40 +00:00
d_preamble_samples [ i ] = 1 ;
2019-01-28 01:29:43 +00:00
}
else
{
2019-07-10 17:52:40 +00:00
d_preamble_samples [ i ] = - 1 ;
2019-01-28 01:29:43 +00:00
}
}
2019-07-10 17:52:40 +00:00
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble ;
d_symbol_history . set_capacity ( d_required_symbols ) ;
2019-03-17 01:57:28 +00:00
2019-07-10 17:52:40 +00:00
d_last_valid_preamble = 0 ;
d_sent_tlm_failed_msg = false ;
d_flag_valid_word = false ;
2018-12-26 18:54:23 +00:00
// Generic settings
2018-12-19 16:20:12 +00:00
d_sample_counter = 0 ;
2018-07-06 12:42:13 +00:00
d_stat = 0 ;
2018-12-19 16:20:12 +00:00
d_preamble_index = 0 ;
2018-07-06 12:42:13 +00:00
d_flag_frame_sync = false ;
2019-07-10 17:52:40 +00:00
d_TOW_at_current_symbol_ms = 0U ;
2019-02-14 21:49:36 +00:00
d_TOW_at_Preamble_ms = 0U ;
2018-12-19 16:20:12 +00:00
Flag_valid_word = false ;
d_CRC_error_counter = 0 ;
2018-07-06 12:42:13 +00:00
d_flag_preamble = false ;
d_channel = 0 ;
2018-12-23 18:51:28 +00:00
flag_SOW_set = false ;
2018-07-06 12:42:13 +00:00
}
2019-03-02 01:21:03 +00:00
beidou_b1i_telemetry_decoder_gs : : ~ beidou_b1i_telemetry_decoder_gs ( )
2018-07-06 12:42:13 +00:00
{
if ( d_dump_file . is_open ( ) = = true )
{
try
{
d_dump_file . close ( ) ;
}
catch ( const std : : exception & ex )
{
LOG ( WARNING ) < < " Exception in destructor closing the dump file " < < ex . what ( ) ;
}
}
}
2018-12-19 16:20:12 +00:00
2019-07-23 17:04:01 +00:00
void beidou_b1i_telemetry_decoder_gs : : decode_bch15_11_01 ( const int32_t * bits , std : : array < int32_t , 15 > & decbits )
2018-12-23 18:51:28 +00:00
{
2019-07-14 12:09:12 +00:00
int32_t bit , err ;
std : : array < int32_t , 4 > reg { 1 , 1 , 1 , 1 } ;
const std : : array < int32_t , 15 > errind { 14 , 13 , 10 , 12 , 6 , 9 , 4 , 11 , 0 , 5 , 7 , 8 , 1 , 3 , 2 } ;
2018-12-23 18:51:28 +00:00
2019-02-10 11:40:03 +00:00
for ( uint32_t i = 0 ; i < 15 ; i + + )
2018-12-23 18:51:28 +00:00
{
decbits [ i ] = bits [ i ] ;
}
2019-02-10 11:40:03 +00:00
for ( uint32_t i = 0 ; i < 15 ; i + + )
2018-12-23 18:51:28 +00:00
{
bit = reg [ 3 ] ;
reg [ 3 ] = reg [ 2 ] ;
reg [ 2 ] = reg [ 1 ] ;
reg [ 1 ] = reg [ 0 ] ;
reg [ 0 ] = bits [ i ] * bit ;
reg [ 1 ] * = bit ;
}
2019-01-28 01:29:43 +00:00
err = errind [ reg [ 0 ] + reg [ 1 ] * 2 + reg [ 2 ] * 4 + reg [ 3 ] * 8 ] ;
2018-12-23 18:51:28 +00:00
2019-07-03 16:57:26 +00:00
if ( err > 0 and err < 16 )
2018-12-23 18:51:28 +00:00
{
decbits [ err - 1 ] * = - 1 ;
}
}
2019-01-25 21:43:00 +00:00
2019-03-02 01:21:03 +00:00
void beidou_b1i_telemetry_decoder_gs : : decode_word (
2019-01-28 01:29:43 +00:00
int32_t word_counter ,
2019-07-10 17:52:40 +00:00
const float * enc_word_symbols ,
2019-01-28 01:29:43 +00:00
int32_t * dec_word_symbols )
2018-12-23 18:51:28 +00:00
{
2019-07-14 12:09:12 +00:00
std : : array < int32_t , 30 > bitsbch { } ;
std : : array < int32_t , 15 > first_branch { } ;
std : : array < int32_t , 15 > second_branch { } ;
2018-12-23 18:51:28 +00:00
if ( word_counter = = 1 )
{
2019-02-10 11:40:03 +00:00
for ( uint32_t j = 0 ; j < 30 ; j + + )
2019-01-28 01:29:43 +00:00
{
2019-02-10 11:40:03 +00:00
dec_word_symbols [ j ] = static_cast < int32_t > ( enc_word_symbols [ j ] > 0 ) ? ( 1 ) : ( - 1 ) ;
2019-01-28 01:29:43 +00:00
}
}
2018-12-23 18:51:28 +00:00
else
{
2019-02-10 11:40:03 +00:00
for ( uint32_t r = 0 ; r < 2 ; r + + )
2019-01-28 01:29:43 +00:00
{
2019-02-10 11:40:03 +00:00
for ( uint32_t c = 0 ; c < 15 ; c + + )
2018-12-23 18:51:28 +00:00
{
2019-02-10 11:40:03 +00:00
bitsbch [ r * 15 + c ] = static_cast < int32_t > ( enc_word_symbols [ c * 2 + r ] > 0 ) ? ( 1 ) : ( - 1 ) ;
2018-12-23 18:51:28 +00:00
}
2019-01-28 01:29:43 +00:00
}
2018-12-23 18:51:28 +00:00
2019-07-23 17:04:01 +00:00
decode_bch15_11_01 ( & bitsbch [ 0 ] , first_branch ) ;
decode_bch15_11_01 ( & bitsbch [ 15 ] , second_branch ) ;
2018-12-23 18:51:28 +00:00
2019-02-10 11:40:03 +00:00
for ( uint32_t j = 0 ; j < 11 ; j + + )
2019-01-28 01:29:43 +00:00
{
dec_word_symbols [ j ] = first_branch [ j ] ;
dec_word_symbols [ j + 11 ] = second_branch [ j ] ;
}
2018-12-23 18:51:28 +00:00
2019-02-10 11:40:03 +00:00
for ( uint32_t j = 0 ; j < 4 ; j + + )
2019-01-28 01:29:43 +00:00
{
dec_word_symbols [ j + 22 ] = first_branch [ 11 + j ] ;
dec_word_symbols [ j + 26 ] = second_branch [ 11 + j ] ;
}
}
2018-12-23 18:51:28 +00:00
}
2019-07-10 17:52:40 +00:00
void beidou_b1i_telemetry_decoder_gs : : decode_subframe ( float * frame_symbols )
2018-12-19 16:20:12 +00:00
{
2019-01-28 01:29:43 +00:00
// 1. Transform from symbols to bits
2018-12-19 16:20:12 +00:00
std : : string data_bits ;
2019-07-14 12:09:12 +00:00
std : : array < int32_t , 30 > dec_word_bits { } ;
2018-12-23 18:51:28 +00:00
// Decode each word in subframe
2019-01-28 01:29:43 +00:00
for ( uint32_t ii = 0 ; ii < BEIDOU_DNAV_WORDS_SUBFRAME ; ii + + )
{
// decode the word
2019-07-14 12:09:12 +00:00
decode_word ( ( ii + 1 ) , & frame_symbols [ ii * 30 ] , dec_word_bits . data ( ) ) ;
2019-01-28 01:29:43 +00:00
// Save word to string format
for ( uint32_t jj = 0 ; jj < ( BEIDOU_DNAV_WORD_LENGTH_BITS ) ; jj + + )
{
data_bits . push_back ( ( dec_word_bits [ jj ] > 0 ) ? ( ' 1 ' ) : ( ' 0 ' ) ) ;
}
}
if ( d_satellite . get_PRN ( ) > 0 and d_satellite . get_PRN ( ) < 6 )
{
d_nav . d2_subframe_decoder ( data_bits ) ;
}
else
{
d_nav . d1_subframe_decoder ( data_bits ) ;
}
2018-12-26 18:54:23 +00:00
2018-12-19 16:20:12 +00:00
// 3. Check operation executed correctly
if ( d_nav . flag_crc_test = = true )
{
2019-07-10 17:52:40 +00:00
DLOG ( INFO ) < < " BeiDou DNAV CRC correct in channel " < < d_channel
< < " from satellite " < < d_satellite ;
2018-12-19 16:20:12 +00:00
}
else
{
2019-07-10 17:52:40 +00:00
DLOG ( INFO ) < < " BeiDou DNAV CRC error in channel " < < d_channel
< < " from satellite " < < d_satellite ;
2018-12-19 16:20:12 +00:00
}
// 4. Push the new navigation data to the queues
if ( d_nav . have_new_ephemeris ( ) = = true )
{
// get object for this SV (mandatory)
2019-01-28 01:29:43 +00:00
std : : shared_ptr < Beidou_Dnav_Ephemeris > tmp_obj = std : : make_shared < Beidou_Dnav_Ephemeris > ( d_nav . get_ephemeris ( ) ) ;
2018-12-19 16:20:12 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
LOG ( INFO ) < < " BEIDOU DNAV Ephemeris have been received in channel " < < d_channel < < " from satellite " < < d_satellite ;
2019-07-04 15:19:33 +00:00
std : : cout < < TEXT_YELLOW < < " New BEIDOU B1I DNAV message received in channel " < < d_channel < < " : ephemeris from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2018-12-19 16:20:12 +00:00
}
if ( d_nav . have_new_utc_model ( ) = = true )
{
// get object for this SV (mandatory)
std : : shared_ptr < Beidou_Dnav_Utc_Model > tmp_obj = std : : make_shared < Beidou_Dnav_Utc_Model > ( d_nav . get_utc_model ( ) ) ;
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
LOG ( INFO ) < < " BEIDOU DNAV UTC Model have been received in channel " < < d_channel < < " from satellite " < < d_satellite ;
2019-07-04 15:19:33 +00:00
std : : cout < < TEXT_YELLOW < < " New BEIDOU B1I DNAV utc model message received in channel " < < d_channel < < " : UTC model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2018-12-19 16:20:12 +00:00
}
if ( d_nav . have_new_iono ( ) = = true )
{
// get object for this SV (mandatory)
std : : shared_ptr < Beidou_Dnav_Iono > tmp_obj = std : : make_shared < Beidou_Dnav_Iono > ( d_nav . get_iono ( ) ) ;
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
LOG ( INFO ) < < " BEIDOU DNAV Iono have been received in channel " < < d_channel < < " from satellite " < < d_satellite ;
2019-07-15 01:09:52 +00:00
std : : cout < < TEXT_YELLOW < < " New BEIDOU B1I DNAV Iono message received in channel " < < d_channel < < " : Iono model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2018-12-19 16:20:12 +00:00
}
if ( d_nav . have_new_almanac ( ) = = true )
{
2019-02-10 11:40:03 +00:00
// uint32_t slot_nbr = d_nav.i_alm_satellite_PRN;
// std::shared_ptr<Beidou_Dnav_Almanac> tmp_obj = std::make_shared<Beidou_Dnav_Almanac>(d_nav.get_almanac(slot_nbr));
// this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
2018-12-23 18:51:28 +00:00
LOG ( INFO ) < < " BEIDOU DNAV Almanac have been received in channel " < < d_channel < < " from satellite " < < d_satellite < < std : : endl ;
2019-07-04 15:19:33 +00:00
std : : cout < < TEXT_YELLOW < < " New BEIDOU B1I DNAV almanac received in channel " < < d_channel < < " from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2018-12-19 16:20:12 +00:00
}
}
2019-03-02 01:21:03 +00:00
void beidou_b1i_telemetry_decoder_gs : : set_satellite ( const Gnss_Satellite & satellite )
2018-07-06 12:42:13 +00:00
{
2019-01-28 01:29:43 +00:00
uint32_t sat_prn = 0 ;
2018-07-06 12:42:13 +00:00
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
DLOG ( INFO ) < < " Setting decoder Finite State Machine to satellite " < < d_satellite ;
DLOG ( INFO ) < < " Navigation Satellite set to " < < d_satellite ;
2018-12-29 01:01:22 +00:00
// Update satellite information for DNAV decoder
sat_prn = d_satellite . get_PRN ( ) ;
d_nav . i_satellite_PRN = sat_prn ;
2019-07-28 10:01:11 +00:00
d_nav . i_signal_type = 1 ; // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
2018-12-29 01:01:22 +00:00
// Update tel dec parameters for D2 NAV Messages
2019-01-28 01:29:43 +00:00
if ( sat_prn > 0 and sat_prn < 6 )
{
2019-03-05 22:00:26 +00:00
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
2019-07-10 17:52:40 +00:00
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ;
2019-07-03 16:57:26 +00:00
2019-01-28 01:29:43 +00:00
// Setting samples of preamble code
for ( int32_t i = 0 ; i < d_symbols_per_preamble ; i + + )
{
if ( BEIDOU_DNAV_PREAMBLE . at ( i ) = = ' 1 ' )
{
2019-07-10 17:52:40 +00:00
d_preamble_samples [ i ] = 1 ;
2019-01-28 01:29:43 +00:00
}
else
{
2019-07-10 17:52:40 +00:00
d_preamble_samples [ i ] = - 1 ;
}
}
d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS ;
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble ;
d_symbol_history . set_capacity ( d_required_symbols ) ;
}
else
{
2019-07-28 10:01:11 +00:00
// back to normal satellites
2019-07-10 17:52:40 +00:00
d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS ;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS ;
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ;
// Setting samples of preamble code
for ( int32_t i = 0 ; i < d_symbols_per_preamble ; i + + )
{
if ( BEIDOU_DNAV_PREAMBLE . at ( i ) = = ' 1 ' )
{
d_preamble_samples [ i ] = 1 ;
}
else
{
d_preamble_samples [ i ] = - 1 ;
2019-01-28 01:29:43 +00:00
}
}
2019-07-10 17:52:40 +00:00
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble ;
d_symbol_history . set_capacity ( d_required_symbols ) ;
2019-01-28 01:29:43 +00:00
}
2018-07-06 12:42:13 +00:00
}
2019-03-02 01:21:03 +00:00
void beidou_b1i_telemetry_decoder_gs : : set_channel ( int32_t channel )
2018-07-06 12:42:13 +00:00
{
d_channel = channel ;
2018-12-19 16:20:12 +00:00
LOG ( INFO ) < < " Navigation channel set to " < < channel ;
2018-07-06 12:42:13 +00:00
// ############# ENABLE DATA FILE LOG #################
if ( d_dump = = true )
{
if ( d_dump_file . is_open ( ) = = false )
{
try
{
d_dump_filename = " telemetry " ;
2019-01-28 01:29:43 +00:00
d_dump_filename . append ( std : : to_string ( d_channel ) ) ;
2018-07-06 12:42:13 +00:00
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 ) ;
2018-12-19 16:20:12 +00:00
LOG ( INFO ) < < " Telemetry decoder dump enabled on channel " < < d_channel < < " Log file: " < < d_dump_filename . c_str ( ) ;
2018-07-06 12:42:13 +00:00
}
catch ( const std : : ifstream : : failure & e )
{
2018-12-19 16:20:12 +00:00
LOG ( WARNING ) < < " channel " < < d_channel < < " : exception opening Beidou TLM dump file. " < < e . what ( ) ;
2018-07-06 12:42:13 +00:00
}
}
}
}
2019-07-10 17:52:40 +00:00
void beidou_b1i_telemetry_decoder_gs : : reset ( )
{
d_last_valid_preamble = d_sample_counter ;
d_TOW_at_current_symbol_ms = 0 ;
d_sent_tlm_failed_msg = false ;
d_flag_valid_word = false ;
DLOG ( INFO ) < < " Beidou B1I Telemetry decoder reset for satellite " < < d_satellite ;
return ;
}
2018-08-06 11:52:08 +00:00
2019-03-02 01:21:03 +00:00
int beidou_b1i_telemetry_decoder_gs : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
2018-07-06 12:42:13 +00:00
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
{
2019-01-28 01:29:43 +00:00
int32_t corr_value = 0 ;
int32_t preamble_diff = 0 ;
2018-12-19 16:20:12 +00:00
2019-01-28 01:29:43 +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-12-19 16:20:12 +00:00
2019-07-20 09:13:28 +00:00
Gnss_Synchro current_symbol { } ; // structure to save the synchronization information and send the output object to the next block
2019-02-10 11:40:03 +00:00
// 1. Copy the current tracking output
2018-07-06 12:42:13 +00:00
current_symbol = in [ 0 ] [ 0 ] ;
2019-02-10 11:40:03 +00:00
d_symbol_history . push_back ( current_symbol . Prompt_I ) ; // add new symbol to the symbol queue
d_sample_counter + + ; // count for the processed samples
2018-07-06 12:42:13 +00:00
consume_each ( 1 ) ;
2018-12-19 16:20:12 +00:00
d_flag_preamble = false ;
2019-07-10 17:52:40 +00:00
if ( d_symbol_history . size ( ) > = d_required_symbols )
2018-08-06 11:52:08 +00:00
{
2019-07-28 10:01:11 +00:00
// ******* preamble correlation ********
2019-02-10 11:40:03 +00:00
for ( int32_t i = 0 ; i < d_samples_per_preamble ; i + + )
2018-08-06 11:52:08 +00:00
{
2019-02-28 12:40:09 +00:00
if ( d_symbol_history [ i ] < 0 ) // symbols clipping
2018-08-06 11:52:08 +00:00
{
2018-12-19 16:20:12 +00:00
corr_value - = d_preamble_samples [ i ] ;
2018-08-06 11:52:08 +00:00
}
else
{
2018-12-19 16:20:12 +00:00
corr_value + = d_preamble_samples [ i ] ;
2018-08-06 11:52:08 +00:00
}
}
2019-07-01 09:00:38 +00:00
}
2019-07-28 10:01:11 +00:00
// ******* frame sync ******************
2019-02-10 11:40:03 +00:00
if ( d_stat = = 0 ) // no preamble information
2018-12-19 16:20:12 +00:00
{
if ( abs ( corr_value ) > = d_samples_per_preamble )
2018-08-06 11:52:08 +00:00
{
2018-12-19 16:20:12 +00:00
// Record the preamble sample stamp
d_preamble_index = d_sample_counter ;
2019-07-10 17:52:40 +00:00
DLOG ( INFO ) < < " Preamble detection for BEIDOU B1I SAT " < < this - > d_satellite ;
2018-12-19 16:20:12 +00:00
// Enter into frame pre-detection status
d_stat = 1 ;
2018-08-06 11:52:08 +00:00
}
}
2018-12-19 16:20:12 +00:00
else if ( d_stat = = 1 ) // possible preamble lock
2018-07-06 12:42:13 +00:00
{
2018-12-19 16:20:12 +00:00
if ( abs ( corr_value ) > = d_samples_per_preamble )
2018-07-06 12:42:13 +00:00
{
2019-02-10 11:40:03 +00:00
// check preamble separation
2018-12-19 16:20:12 +00:00
preamble_diff = static_cast < int32_t > ( d_sample_counter - d_preamble_index ) ;
if ( abs ( preamble_diff - d_preamble_period_samples ) = = 0 )
{
2019-02-10 11:40:03 +00:00
// try to decode frame
2019-07-10 17:52:40 +00:00
DLOG ( INFO ) < < " Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " < < this - > d_satellite ;
2019-07-28 10:01:11 +00:00
d_preamble_index = d_sample_counter ; // record the preamble sample stamp
2019-07-10 17:52:40 +00:00
2018-12-19 16:20:12 +00:00
d_stat = 2 ;
2019-07-10 17:52:40 +00:00
// ******* SAMPLES TO SYMBOLS *******
2019-07-28 10:01:11 +00:00
if ( corr_value > 0 ) // normal PLL lock
2019-07-10 17:52:40 +00:00
{
for ( uint32_t i = 0 ; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ; i + + )
{
d_subframe_symbols [ i ] = d_symbol_history . at ( i ) ;
}
}
else // 180 deg. inverted carrier phase PLL lock
{
for ( uint32_t i = 0 ; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ; i + + )
{
d_subframe_symbols [ i ] = - d_symbol_history . at ( i ) ;
}
}
// call the decoder
2019-07-19 16:23:36 +00:00
decode_subframe ( d_subframe_symbols . data ( ) ) ;
2019-07-10 17:52:40 +00:00
if ( d_nav . flag_crc_test = = true )
{
d_CRC_error_counter = 0 ;
d_flag_preamble = true ; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter ; // record the preamble sample stamp (t_P)
if ( ! d_flag_frame_sync )
{
d_flag_frame_sync = true ;
DLOG ( INFO ) < < " BeiDou DNAV frame sync found for SAT " < < this - > d_satellite ;
}
}
else
{
d_CRC_error_counter + + ;
d_preamble_index = d_sample_counter ; // record the preamble sample stamp
if ( d_CRC_error_counter > CRC_ERROR_LIMIT )
{
DLOG ( INFO ) < < " BeiDou DNAV frame sync lost for SAT " < < this - > d_satellite ;
d_flag_frame_sync = false ;
d_stat = 0 ;
flag_SOW_set = false ;
}
}
2018-12-19 16:20:12 +00:00
}
else
{
if ( preamble_diff > d_preamble_period_samples )
2018-07-06 12:42:13 +00:00
{
2018-12-19 16:20:12 +00:00
d_stat = 0 ; // start again
2018-07-06 12:42:13 +00:00
}
2018-12-19 16:20:12 +00:00
DLOG ( INFO ) < < " Failed BeiDou DNAV frame decoding for BeiDou B1I SAT " < < this - > d_satellite ;
}
2018-07-06 12:42:13 +00:00
}
}
2019-01-28 01:29:43 +00:00
else if ( d_stat = = 2 ) // preamble acquired
2018-07-06 12:42:13 +00:00
{
2019-01-28 01:29:43 +00:00
if ( d_sample_counter = = d_preamble_index + static_cast < uint64_t > ( d_preamble_period_samples ) )
2018-07-06 12:42:13 +00:00
{
2019-02-10 11:40:03 +00:00
// ******* SAMPLES TO SYMBOLS *******
2019-07-28 10:01:11 +00:00
if ( corr_value > 0 ) // normal PLL lock
2018-07-06 12:42:13 +00:00
{
2019-02-10 11:40:03 +00:00
for ( uint32_t i = 0 ; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ; i + + )
2018-07-06 12:42:13 +00:00
{
2019-07-10 17:52:40 +00:00
d_subframe_symbols [ i ] = d_symbol_history . at ( i ) ;
2018-12-19 16:20:12 +00:00
}
}
2019-02-10 11:40:03 +00:00
else // 180 deg. inverted carrier phase PLL lock
2018-12-19 16:20:12 +00:00
{
2019-02-10 11:40:03 +00:00
for ( uint32_t i = 0 ; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS ; i + + )
2018-12-19 16:20:12 +00:00
{
2019-07-10 17:52:40 +00:00
d_subframe_symbols [ i ] = - d_symbol_history . at ( i ) ;
2018-07-06 12:42:13 +00:00
}
}
2018-12-19 16:20:12 +00:00
2019-02-10 11:40:03 +00:00
// call the decoder
2019-07-19 16:23:36 +00:00
decode_subframe ( d_subframe_symbols . data ( ) ) ;
2018-12-26 18:54:23 +00:00
2018-12-19 16:20:12 +00:00
if ( d_nav . flag_crc_test = = true )
2018-07-06 12:42:13 +00:00
{
2018-12-19 16:20:12 +00:00
d_CRC_error_counter = 0 ;
2019-02-10 11:40:03 +00:00
d_flag_preamble = true ; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter ; // record the preamble sample stamp (t_P)
2018-12-19 16:20:12 +00:00
if ( ! d_flag_frame_sync )
{
d_flag_frame_sync = true ;
DLOG ( INFO ) < < " BeiDou DNAV frame sync found for SAT " < < this - > d_satellite ;
}
2018-07-06 12:42:13 +00:00
}
2018-12-19 16:20:12 +00:00
else
{
d_CRC_error_counter + + ;
2019-02-10 11:40:03 +00:00
d_preamble_index = d_sample_counter ; // record the preamble sample stamp
2018-12-19 16:20:12 +00:00
if ( d_CRC_error_counter > CRC_ERROR_LIMIT )
2018-07-06 12:42:13 +00:00
{
2019-07-10 17:52:40 +00:00
DLOG ( INFO ) < < " BeiDou DNAV frame sync lost for SAT " < < this - > d_satellite ;
2018-12-19 16:20:12 +00:00
d_flag_frame_sync = false ;
d_stat = 0 ;
2018-12-23 18:51:28 +00:00
flag_SOW_set = false ;
2018-07-06 12:42:13 +00:00
}
2018-12-19 16:20:12 +00:00
}
2018-07-06 12:42:13 +00:00
}
2018-12-19 16:20:12 +00:00
}
// UPDATE GNSS SYNCHRO DATA
2019-02-10 11:40:03 +00:00
// 2. Add the telemetry decoder information
2018-12-23 18:51:28 +00:00
if ( this - > d_flag_preamble = = true and d_nav . flag_new_SOW_available = = true )
2019-02-10 11:40:03 +00:00
// update TOW at the preamble instant
2018-07-06 12:42:13 +00:00
{
2019-01-28 01:29:43 +00:00
// Reporting sow as gps time of week
2019-07-10 15:49:41 +00:00
d_TOW_at_Preamble_ms = static_cast < uint32_t > ( ( d_nav . d_SOW + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET ) * 1000.0 ) ;
2019-07-28 10:01:11 +00:00
// check TOW update consistency
2019-07-10 17:52:40 +00:00
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms ;
2019-07-28 10:01:11 +00:00
// compute new TOW
2019-07-10 17:52:40 +00:00
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms ;
2019-01-28 01:29:43 +00:00
flag_SOW_set = true ;
d_nav . flag_new_SOW_available = false ;
2018-07-06 12:42:13 +00:00
2019-07-10 17:52:40 +00:00
if ( last_d_TOW_at_current_symbol_ms ! = 0 and abs ( static_cast < int64_t > ( d_TOW_at_current_symbol_ms ) - int64_t ( last_d_TOW_at_current_symbol_ms ) ) > d_symbol_duration_ms )
{
LOG ( INFO ) < < " Warning: BEIDOU B1I TOW update in ch " < < d_channel
< < " does not match the TLM TOW counter " < < static_cast < int64_t > ( d_TOW_at_current_symbol_ms ) - int64_t ( last_d_TOW_at_current_symbol_ms ) < < " ms \n " ;
2018-07-06 12:42:13 +00:00
2019-07-10 17:52:40 +00:00
d_TOW_at_current_symbol_ms = 0 ;
d_flag_valid_word = false ;
}
else
{
d_last_valid_preamble = d_sample_counter ;
d_flag_valid_word = true ;
}
2018-12-19 16:20:12 +00:00
}
else
2018-07-06 12:42:13 +00:00
{
2019-07-10 17:52:40 +00:00
if ( d_flag_valid_word )
{
d_TOW_at_current_symbol_ms + = d_symbol_duration_ms ;
if ( current_symbol . Flag_valid_symbol_output = = false )
{
d_flag_valid_word = false ;
}
}
2018-07-06 12:42:13 +00:00
}
2019-07-10 17:52:40 +00:00
if ( d_flag_valid_word = = true )
2019-07-05 18:30:34 +00:00
{
2019-07-10 17:52:40 +00:00
current_symbol . TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms ;
current_symbol . Flag_valid_word = d_flag_valid_word ;
if ( d_dump = = true )
2019-07-05 18:30:34 +00:00
{
2019-07-10 17:52:40 +00:00
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double ;
uint64_t tmp_ulong_int ;
tmp_double = static_cast < double > ( d_TOW_at_current_symbol_ms ) / 1000.0 ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
tmp_ulong_int = current_symbol . Tracking_sample_counter ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_ulong_int ) , sizeof ( uint64_t ) ) ;
tmp_double = static_cast < double > ( d_TOW_at_Preamble_ms ) / 1000.0 ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
}
catch ( const std : : ifstream : : failure & e )
{
LOG ( WARNING ) < < " Exception writing Telemetry GPS L5 dump file " < < e . what ( ) ;
}
2018-07-06 12:42:13 +00:00
}
2019-07-05 18:30:34 +00:00
2019-07-10 17:52:40 +00:00
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
* out [ 0 ] = current_symbol ;
return 1 ;
}
return 0 ;
2018-07-06 12:42:13 +00:00
}