2017-11-29 14:51:30 +00:00
/*!
2019-03-02 01:21:03 +00:00
* \ file gps_l5_telemetry_decoder_gs . cc
2018-04-22 21:07:34 +00:00
* \ brief Implementation of a CNAV message demodulator block
2017-11-29 14:51:30 +00:00
* \ author Antonio Ramos , 2017. antonio . ramos ( at ) cttc . es
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2019-07-26 10:38:20 +00:00
* Copyright ( C ) 2010 - 2019 ( see AUTHORS file for a list of contributors )
2017-11-29 14:51:30 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2017-11-29 14:51:30 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
2018-02-26 02:15:53 +00:00
2019-03-03 12:39:35 +00:00
# include "gps_l5_telemetry_decoder_gs.h"
2018-04-22 21:07:34 +00:00
# include "display.h"
2018-02-26 02:15:53 +00:00
# include "gnss_synchro.h"
2017-12-11 11:17:01 +00:00
# include "gps_cnav_ephemeris.h"
# include "gps_cnav_iono.h"
2019-03-05 21:17:09 +00:00
# include "gps_cnav_utc_model.h" // for Gps_CNAV_Utc_Model
2018-04-22 21:07:34 +00:00
# include <glog/logging.h>
# 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
2020-03-16 10:51:50 +00:00
# include <bitset> // for std::bitset
# include <cstdlib> // for std::llabs
# include <exception> // for std::exception
# include <iostream> // for std::cout
# include <memory> // for shared_ptr, make_shared
2018-02-26 02:15:53 +00:00
2017-11-29 14:51:30 +00:00
2019-03-02 01:21:03 +00:00
gps_l5_telemetry_decoder_gs_sptr
gps_l5_make_telemetry_decoder_gs ( const Gnss_Satellite & satellite , bool dump )
2017-11-29 14:51:30 +00:00
{
2019-03-02 01:21:03 +00:00
return gps_l5_telemetry_decoder_gs_sptr ( new gps_l5_telemetry_decoder_gs ( satellite , dump ) ) ;
2017-11-29 14:51:30 +00:00
}
2019-03-02 01:21:03 +00:00
gps_l5_telemetry_decoder_gs : : gps_l5_telemetry_decoder_gs (
const Gnss_Satellite & satellite , bool dump ) : gr : : block ( " gps_l5_telemetry_decoder_gs " ,
2018-03-03 01:03:39 +00:00
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2017-11-29 14:51:30 +00:00
{
2019-07-20 09:13:28 +00:00
// prevent telemetry symbols accumulation in output buffers
2019-07-04 15:19:33 +00:00
this - > set_max_noutput_items ( 1 ) ;
2017-11-29 14:51:30 +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 " ) ) ;
d_last_valid_preamble = 0 ;
d_sent_tlm_failed_msg = false ;
2019-07-20 09:13:28 +00:00
d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SYMBOLS_PER_BIT * 10 ; // rise alarm if 20 consecutive subframes have no valid CRC
2019-03-18 15:28:49 +00:00
2017-11-29 14:51:30 +00:00
// initialize internal vars
d_dump = dump ;
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
DLOG ( INFO ) < < " GPS L5 TELEMETRY PROCESSING: satellite " < < d_satellite ;
d_channel = 0 ;
d_flag_valid_word = false ;
2018-08-17 16:10:53 +00:00
d_TOW_at_current_symbol_ms = 0U ;
d_TOW_at_Preamble_ms = 0U ;
2018-08-13 11:03:01 +00:00
// initialize the CNAV frame decoder (libswiftcnav)
2017-11-29 14:51:30 +00:00
cnav_msg_decoder_init ( & d_cnav_decoder ) ;
2019-03-18 15:28:49 +00:00
d_sample_counter = 0 ;
2020-06-19 00:15:56 +00:00
d_flag_PLL_180_deg_phase_locked = false ;
2017-11-29 14:51:30 +00:00
}
2019-03-02 01:21:03 +00:00
gps_l5_telemetry_decoder_gs : : ~ gps_l5_telemetry_decoder_gs ( )
2017-11-29 14:51:30 +00:00
{
2020-06-25 00:50:07 +00:00
DLOG ( INFO ) < < " GPS L5 Telemetry decoder block (channel " < < d_channel < < " ) destructor called. " ;
2018-03-03 01:03:39 +00:00
if ( d_dump_file . is_open ( ) = = true )
2017-11-29 14:51:30 +00:00
{
try
2018-03-03 01:03:39 +00:00
{
2017-11-29 14:51:30 +00:00
d_dump_file . close ( ) ;
2018-03-03 01:03:39 +00:00
}
catch ( const std : : exception & ex )
{
2017-11-29 14:51:30 +00:00
LOG ( WARNING ) < < " Exception in destructor closing the dump file " < < ex . what ( ) ;
2018-03-03 01:03:39 +00:00
}
2017-11-29 14:51:30 +00:00
}
}
2019-03-02 01:21:03 +00:00
void gps_l5_telemetry_decoder_gs : : set_satellite ( const Gnss_Satellite & satellite )
2018-04-13 09:07:58 +00:00
{
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
2019-03-01 15:48:10 +00:00
DLOG ( INFO ) < < " GPS L5 CNAV telemetry decoder in channel " < < this - > d_channel < < " set to satellite " < < d_satellite ;
2020-06-10 08:15:59 +00:00
d_CNAV_Message = Gps_CNAV_Navigation_Message ( ) ;
2018-04-13 09:07:58 +00:00
}
2019-03-02 01:21:03 +00:00
void gps_l5_telemetry_decoder_gs : : set_channel ( int32_t channel )
2018-04-13 09:07:58 +00:00
{
d_channel = channel ;
2020-06-10 08:15:59 +00:00
d_CNAV_Message = Gps_CNAV_Navigation_Message ( ) ;
2019-03-01 15:48:10 +00:00
DLOG ( INFO ) < < " GPS L5 CNAV channel set to " < < channel ;
2018-04-13 09:07:58 +00:00
// ############# ENABLE DATA FILE LOG #################
if ( d_dump = = true )
{
if ( d_dump_file . is_open ( ) = = false )
{
try
{
d_dump_filename = " telemetry_L5_ " ;
2018-12-03 22:34:29 +00:00
d_dump_filename . append ( std : : to_string ( d_channel ) ) ;
2018-04-13 09:07:58 +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 ) ;
LOG ( INFO ) < < " Telemetry decoder dump enabled on channel " < < d_channel
< < " Log file: " < < d_dump_filename . c_str ( ) ;
}
catch ( const std : : ifstream : : failure & e )
{
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening Telemetry GPS L5 dump file " < < e . what ( ) ;
}
}
}
}
2019-03-18 18:41:41 +00:00
2019-03-18 15:28:49 +00:00
void gps_l5_telemetry_decoder_gs : : reset ( )
{
d_last_valid_preamble = d_sample_counter ;
2019-06-21 16:06:48 +00:00
d_TOW_at_current_symbol_ms = 0 ;
2019-03-18 15:28:49 +00:00
d_sent_tlm_failed_msg = false ;
2019-06-21 16:06:48 +00:00
d_flag_valid_word = false ;
2019-03-18 15:28:49 +00:00
DLOG ( INFO ) < < " Telemetry decoder reset for satellite " < < d_satellite ;
}
2018-04-13 09:07:58 +00:00
2019-03-02 01:21:03 +00:00
int gps_l5_telemetry_decoder_gs : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
2018-03-03 01:03:39 +00:00
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
2017-11-29 14:51:30 +00:00
{
// 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
2017-11-29 14:51:30 +00:00
// UPDATE GNSS SYNCHRO DATA
2019-02-28 12:40:09 +00:00
Gnss_Synchro current_synchro_data { } ; // structure to save the synchronization information and send the output object to the next block
2018-08-13 11:03:01 +00:00
// 1. Copy the current tracking output
2017-11-29 14:51:30 +00:00
current_synchro_data = in [ 0 ] ;
2019-02-28 12:40:09 +00:00
consume_each ( 1 ) ; // one by one
2017-11-29 14:51:30 +00:00
2019-03-18 15:28:49 +00:00
// check if there is a problem with the telemetry of the current satellite
d_sample_counter + + ; // count for the processed symbols
if ( d_sent_tlm_failed_msg = = false )
{
if ( ( d_sample_counter - d_last_valid_preamble ) > d_max_symbols_without_valid_frame )
{
2019-07-28 10:01:11 +00:00
int message = 1 ; // bad telemetry
2019-03-18 15:28:49 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry_to_trk " ) , pmt : : make_any ( message ) ) ;
d_sent_tlm_failed_msg = true ;
}
}
2017-12-11 11:17:01 +00:00
cnav_msg_t msg ;
2019-07-01 09:00:38 +00:00
uint32_t delay ;
uint8_t symbol_clip = static_cast < uint8_t > ( current_synchro_data . Prompt_Q > 0 ) * 255 ;
2018-08-13 11:03:01 +00:00
// 2. Add the telemetry decoder information
// check if new CNAV frame is available
2019-07-01 09:00:38 +00:00
if ( cnav_msg_decoder_add_symbol ( & d_cnav_decoder , symbol_clip , & msg , & delay ) = = true )
2017-11-29 14:51:30 +00:00
{
2020-03-16 10:51:50 +00:00
if ( d_cnav_decoder . part1 . invert = = true or d_cnav_decoder . part2 . invert = = true )
2020-03-09 10:38:48 +00:00
{
2020-06-19 00:15:56 +00:00
d_flag_PLL_180_deg_phase_locked = true ;
2020-03-09 10:38:48 +00:00
}
else
{
2020-06-19 00:15:56 +00:00
d_flag_PLL_180_deg_phase_locked = false ;
2020-03-09 10:38:48 +00:00
}
2017-11-29 14:51:30 +00:00
std : : bitset < GPS_L5_CNAV_DATA_PAGE_BITS > raw_bits ;
2018-08-13 11:03:01 +00:00
// Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder
for ( uint32_t i = 0 ; i < GPS_L5_CNAV_DATA_PAGE_BITS ; i + + )
2017-11-29 14:51:30 +00:00
{
2019-07-14 00:28:28 +00:00
raw_bits [ GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i ] = ( ( msg . raw_msg [ i / 8 ] > > ( 7 - i % 8 ) ) & 1U ) ;
2017-11-29 14:51:30 +00:00
}
d_CNAV_Message . decode_page ( raw_bits ) ;
2018-08-13 11:03:01 +00:00
// Push the new navigation data to the queues
2017-11-29 14:51:30 +00:00
if ( d_CNAV_Message . have_new_ephemeris ( ) = = true )
{
// get ephemeris object for this SV
std : : shared_ptr < Gps_CNAV_Ephemeris > tmp_obj = std : : make_shared < Gps_CNAV_Ephemeris > ( d_CNAV_Message . get_ephemeris ( ) ) ;
2018-04-22 21:07:34 +00:00
std : : cout < < TEXT_MAGENTA < < " New GPS L5 CNAV message received in channel " < < d_channel < < " : ephemeris from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2017-11-29 14:51:30 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
}
if ( d_CNAV_Message . have_new_iono ( ) = = true )
{
std : : shared_ptr < Gps_CNAV_Iono > tmp_obj = std : : make_shared < Gps_CNAV_Iono > ( d_CNAV_Message . get_iono ( ) ) ;
2018-04-22 21:07:34 +00:00
std : : cout < < TEXT_MAGENTA < < " New GPS L5 CNAV message received in channel " < < d_channel < < " : iono model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2017-11-29 14:51:30 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
}
if ( d_CNAV_Message . have_new_utc_model ( ) = = true )
{
std : : shared_ptr < Gps_CNAV_Utc_Model > tmp_obj = std : : make_shared < Gps_CNAV_Utc_Model > ( d_CNAV_Message . get_utc_model ( ) ) ;
2018-04-22 21:07:34 +00:00
std : : cout < < TEXT_MAGENTA < < " New GPS L5 CNAV message received in channel " < < d_channel < < " : UTC model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
2017-11-29 14:51:30 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
}
2018-08-13 11:03:01 +00:00
// update TOW at the preamble instant
2018-08-07 17:39:56 +00:00
d_TOW_at_Preamble_ms = msg . tow * 6000 ;
2019-06-21 16:06:48 +00:00
2018-08-13 11:03:01 +00:00
// The time of the last input symbol can be computed from the message ToW and
// delay by the formulae:
// \code
// symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory
2019-06-21 16:06:48 +00:00
2019-07-20 09:13:28 +00:00
// check TOW update consistency
2019-06-21 16:06:48 +00:00
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms ;
2019-02-22 09:47:24 +00:00
d_TOW_at_current_symbol_ms = msg . tow * 6000 + ( delay + 12 ) * GPS_L5I_SYMBOL_PERIOD_MS ;
2020-03-16 10:51:50 +00:00
if ( last_d_TOW_at_current_symbol_ms ! = 0 and std : : llabs ( static_cast < int64_t > ( d_TOW_at_current_symbol_ms ) - static_cast < int64_t > ( last_d_TOW_at_current_symbol_ms ) ) > static_cast < int64_t > ( GPS_L5I_SYMBOL_PERIOD_MS ) )
2019-06-21 16:06:48 +00:00
{
DLOG ( INFO ) < < " Warning: GPS L5 TOW update in ch " < < d_channel
2020-03-16 10:51:50 +00:00
< < " does not match the TLM TOW counter " < < static_cast < int64_t > ( d_TOW_at_current_symbol_ms ) - static_cast < int64_t > ( last_d_TOW_at_current_symbol_ms ) < < " ms "
2019-06-21 16:06:48 +00:00
< < " with delay: " < < delay < < " msg tow: " < < msg . tow * 6000 < < " ms \n " ;
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 ;
}
2017-11-29 14:51:30 +00:00
}
else
{
2019-06-21 16:06:48 +00:00
if ( d_flag_valid_word )
2017-11-29 14:51:30 +00:00
{
2019-07-01 09:00:38 +00:00
d_TOW_at_current_symbol_ms + = GPS_L5I_SYMBOL_PERIOD_MS ;
2019-06-21 16:06:48 +00:00
if ( current_synchro_data . Flag_valid_symbol_output = = false )
{
d_flag_valid_word = false ;
}
2017-11-29 14:51:30 +00:00
}
}
2018-08-07 17:39:56 +00:00
if ( d_flag_valid_word = = true )
2017-11-29 14:51:30 +00:00
{
2020-06-19 00:15:56 +00:00
if ( d_flag_PLL_180_deg_phase_locked = = true )
2020-03-09 10:38:48 +00:00
{
// correct the accumulated phase for the Costas loop phase shift, if required
current_synchro_data . Carrier_phase_rads + = GPS_L5_PI ;
}
2018-08-07 17:39:56 +00:00
current_synchro_data . TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms ;
current_synchro_data . Flag_valid_word = d_flag_valid_word ;
if ( d_dump = = true )
2018-03-03 01:03:39 +00:00
{
2018-08-07 17:39:56 +00:00
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double ;
2018-08-10 18:34:03 +00:00
uint64_t tmp_ulong_int ;
2018-08-07 17:39:56 +00:00
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_synchro_data . Tracking_sample_counter ;
2018-08-10 18:34:03 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_ulong_int ) , sizeof ( uint64_t ) ) ;
2018-08-07 17:39:56 +00:00
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-03-03 01:03:39 +00:00
}
2017-11-29 14:51:30 +00:00
2018-08-13 11:03:01 +00:00
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
2018-08-07 17:39:56 +00:00
out [ 0 ] = current_synchro_data ;
return 1 ;
}
2018-12-03 21:08:19 +00:00
return 0 ;
2017-11-29 14:51:30 +00:00
}