2015-05-05 16:18:16 +00:00
/*!
2019-03-02 01:21:03 +00:00
* \ file gps_l2c_telemetry_decoder_gs . cc
2019-07-20 09:13:28 +00:00
* \ brief Implementation of a NAV message demodulator block
2015-05-05 16:18:16 +00:00
* \ author Javier Arribas , 2015. jarribas ( at ) cttc . es
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2015-05-05 16:18:16 +00:00
*
2020-12-30 12:35:06 +00:00
* GNSS - SDR is a Global Navigation Satellite System software - defined receiver .
2015-05-05 16:18:16 +00:00
* This file is part of GNSS - SDR .
*
2020-12-30 12:35:06 +00:00
* Copyright ( C ) 2010 - 2020 ( see AUTHORS file for a list of contributors )
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2015-05-05 16:18:16 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2015-05-05 16:18:16 +00:00
*/
2018-02-26 02:15:53 +00:00
2019-03-03 12:39:35 +00:00
# include "gps_l2c_telemetry_decoder_gs.h"
2019-03-05 21:17:09 +00:00
# include "GPS_L2C.h" // for GPS_L2_CNAV_DATA_PAGE_BITS, GPS_L...
2018-02-26 11:33:40 +00:00
# include "display.h"
2021-09-13 21:19:37 +00:00
# include "gnss_sdr_make_unique.h" // for std::make_unique in C++11
2018-12-09 21:00:09 +00:00
# include "gnss_synchro.h"
2019-03-05 21:17:09 +00:00
# include "gps_cnav_ephemeris.h" // for Gps_CNAV_Ephemeris
# include "gps_cnav_iono.h" // for Gps_CNAV_Iono
# include "gps_cnav_utc_model.h" // for Gps_CNAV_Utc_Model
2020-11-23 13:42:13 +00:00
# include "tlm_utils.h"
2018-02-26 02:15:53 +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
# include <bitset> // for bitset
# include <cmath> // for round
2020-11-21 12:14:55 +00:00
# include <cstddef> // for size_t
2019-03-05 21:17:09 +00:00
# include <exception> // for exception
# include <iostream> // for cout
# include <memory> // for shared_ptr, make_shared
2018-02-26 02:15:53 +00:00
2015-05-05 16:18:16 +00:00
2019-03-02 01:21:03 +00:00
gps_l2c_telemetry_decoder_gs_sptr
2020-11-21 18:37:22 +00:00
gps_l2c_make_telemetry_decoder_gs ( const Gnss_Satellite & satellite , const Tlm_Conf & conf )
2015-05-05 16:18:16 +00:00
{
2020-11-21 18:37:22 +00:00
return gps_l2c_telemetry_decoder_gs_sptr ( new gps_l2c_telemetry_decoder_gs ( satellite , conf ) ) ;
2015-05-05 16:18:16 +00:00
}
2019-03-02 01:21:03 +00:00
gps_l2c_telemetry_decoder_gs : : gps_l2c_telemetry_decoder_gs (
2020-11-21 18:37:22 +00:00
const Gnss_Satellite & satellite ,
const Tlm_Conf & conf ) : gr : : block ( " gps_l2c_telemetry_decoder_gs " ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ,
2021-10-10 16:56:14 +00:00
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ) ,
d_dump_filename ( conf . dump_filename ) ,
d_TOW_at_current_symbol ( 0 ) ,
d_TOW_at_Preamble ( 0 ) ,
d_sample_counter ( 0 ) ,
d_last_valid_preamble ( 0 ) ,
d_channel ( 0 ) ,
d_dump ( conf . dump ) ,
d_sent_tlm_failed_msg ( false ) ,
d_flag_PLL_180_deg_phase_locked ( false ) ,
d_flag_valid_word ( false ) ,
d_dump_mat ( conf . dump_mat ) ,
d_remove_dat ( conf . remove_dat ) ,
d_enable_navdata_monitor ( conf . enable_navdata_monitor ) ,
d_dump_crc_stats ( conf . dump_crc_stats )
2015-05-05 16:18:16 +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 ) ;
2016-04-12 15:29:16 +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 " ) ) ;
2021-10-10 16:56:14 +00:00
2021-09-05 22:05:29 +00:00
if ( d_enable_navdata_monitor )
{
// register nav message monitor out
this - > message_port_register_out ( pmt : : mp ( " Nav_msg_from_TLM " ) ) ;
d_nav_msg_packet . system = std : : string ( " G " ) ;
d_nav_msg_packet . signal = std : : string ( " 2S " ) ;
}
2019-07-20 09:13:28 +00:00
d_max_symbols_without_valid_frame = GPS_L2_CNAV_DATA_PAGE_BITS * GPS_L2_SYMBOLS_PER_BIT * 5 ; // rise alarm if 5 consecutive subframes have no valid CRC
2019-03-18 15:28:49 +00:00
2015-05-05 16:18:16 +00:00
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
2017-03-23 14:45:41 +00:00
DLOG ( INFO ) < < " GPS L2C M TELEMETRY PROCESSING: satellite " < < d_satellite ;
2018-08-13 11:03:01 +00:00
// initialize the CNAV frame decoder (libswiftcnav)
2017-03-23 14:45:41 +00:00
cnav_msg_decoder_init ( & d_cnav_decoder ) ;
2019-03-18 15:28:49 +00:00
2021-09-13 21:19:37 +00:00
if ( d_dump_crc_stats )
{
// initialize the telemetry CRC statistics class
d_Tlm_CRC_Stats = std : : make_unique < Tlm_CRC_Stats > ( ) ;
d_Tlm_CRC_Stats - > initialize ( conf . dump_crc_stats_filename ) ;
}
else
{
d_Tlm_CRC_Stats = nullptr ;
}
2015-05-05 16:18:16 +00:00
}
2019-03-02 01:21:03 +00:00
gps_l2c_telemetry_decoder_gs : : ~ gps_l2c_telemetry_decoder_gs ( )
2015-05-05 16:18:16 +00:00
{
2020-06-25 00:50:07 +00:00
DLOG ( INFO ) < < " GPS L2C Telemetry decoder block (channel " < < d_channel < < " ) destructor called. " ;
2020-11-21 12:14:55 +00:00
size_t pos = 0 ;
2018-03-03 01:03:39 +00:00
if ( d_dump_file . is_open ( ) = = true )
2017-07-28 22:25:43 +00:00
{
2020-11-21 12:14:55 +00:00
pos = d_dump_file . tellp ( ) ;
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
}
2020-11-21 12:14:55 +00:00
if ( pos = = 0 )
{
2020-11-23 13:42:13 +00:00
if ( ! tlm_remove_file ( d_dump_filename ) )
2020-11-21 12:14:55 +00:00
{
LOG ( WARNING ) < < " Error deleting temporary file " ;
}
}
2017-07-28 22:25:43 +00:00
}
2020-11-21 18:37:22 +00:00
if ( d_dump & & ( pos ! = 0 ) & & d_dump_mat )
2020-11-19 08:55:08 +00:00
{
2020-11-23 13:42:13 +00:00
save_tlm_matfile ( d_dump_filename ) ;
2020-11-23 15:09:27 +00:00
if ( d_remove_dat )
{
if ( ! tlm_remove_file ( d_dump_filename ) )
{
LOG ( WARNING ) < < " Error deleting temporary file " ;
}
}
2020-11-19 08:55:08 +00:00
}
2015-05-05 16:18:16 +00:00
}
2019-03-02 01:21:03 +00:00
void gps_l2c_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 ( ) ) ;
DLOG ( INFO ) < < " GPS L2C CNAV telemetry decoder in channel " < < this - > d_channel < < " set to satellite " < < d_satellite ;
}
2019-03-02 01:21:03 +00:00
void gps_l2c_telemetry_decoder_gs : : set_channel ( int channel )
2018-04-13 09:07:58 +00:00
{
d_channel = channel ;
LOG ( INFO ) < < " GPS L2C CNAV channel set to " < < channel ;
// ############# ENABLE DATA FILE LOG #################
if ( d_dump = = true )
{
if ( d_dump_file . is_open ( ) = = false )
{
try
{
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 L2 dump file " < < e . what ( ) ;
}
}
}
2021-09-13 21:19:37 +00:00
if ( d_dump_crc_stats )
{
// set the channel number for the telemetry CRC statistics
// disable the telemetry CRC statistics if there is a problem opening the output file
d_dump_crc_stats = d_Tlm_CRC_Stats - > set_channel ( d_channel ) ;
}
2018-04-13 09:07:58 +00:00
}
2019-03-18 15:28:49 +00:00
void gps_l2c_telemetry_decoder_gs : : reset ( )
{
d_last_valid_preamble = d_sample_counter ;
d_sent_tlm_failed_msg = false ;
DLOG ( INFO ) < < " Telemetry decoder reset for satellite " < < d_satellite ;
}
2019-03-18 18:41:41 +00:00
2019-03-02 01:21:03 +00:00
int gps_l2c_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 )
2015-05-05 16:18:16 +00:00
{
2015-05-14 16:53:12 +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
2015-05-14 16:53:12 +00:00
2015-06-05 23:21:00 +00:00
bool flag_new_cnav_frame = false ;
2017-03-23 14:45:41 +00:00
cnav_msg_t msg ;
2018-08-13 11:03:01 +00:00
uint32_t delay = 0 ;
2015-05-05 16:18:16 +00:00
2018-08-13 11:03:01 +00:00
// add the symbol to the decoder
2020-07-19 07:39:32 +00:00
const uint8_t symbol_clip = static_cast < uint8_t > ( in [ 0 ] . Prompt_I > 0 ) * 255 ;
2017-03-23 14:45:41 +00:00
flag_new_cnav_frame = cnav_msg_decoder_add_symbol ( & d_cnav_decoder , symbol_clip , & msg , & delay ) ;
2021-09-13 21:19:37 +00:00
if ( d_dump_crc_stats & & ( d_cnav_decoder . part1 . message_lock | | d_cnav_decoder . part2 . message_lock ) )
{
// update CRC statistics
d_Tlm_CRC_Stats - > update_CRC_stats ( ( d_cnav_decoder . part1 . crc_ok | | d_cnav_decoder . part2 . crc_ok ) ) ;
d_cnav_decoder . part1 . message_lock = false ;
d_cnav_decoder . part2 . message_lock = false ;
}
2015-05-05 16:18:16 +00:00
2018-08-13 11:03:01 +00:00
consume_each ( 1 ) ; // one by one
2015-05-05 16:18:16 +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 )
{
2020-07-27 23:23:27 +00:00
const 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 ;
}
}
2015-05-14 16:53:12 +00:00
// UPDATE GNSS SYNCHRO DATA
2018-12-08 17:49:31 +00:00
Gnss_Synchro current_synchro_data { } ; // structure to save the synchronization information and send the output object to the next block
2015-06-05 16:46:00 +00:00
2018-08-13 11:03:01 +00:00
// 1. Copy the current tracking output
2015-06-05 16:46:00 +00:00
current_synchro_data = in [ 0 ] ;
2018-08-13 11:03:01 +00:00
// 2. Add the telemetry decoder information
// check if new CNAV frame is available
2017-03-23 14:45:41 +00:00
if ( flag_new_cnav_frame = = true )
2017-06-12 17:22:52 +00:00
{
2021-09-13 21:19:37 +00:00
if ( d_cnav_decoder . part1 . invert = = true | | 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-06-12 17:22:52 +00:00
std : : bitset < GPS_L2_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_L2_CNAV_DATA_PAGE_BITS ; i + + )
2017-06-12 17:22:52 +00:00
{
2019-06-29 12:22:15 +00:00
raw_bits [ GPS_L2_CNAV_DATA_PAGE_BITS - 1 - i ] = ( ( msg . raw_msg [ i / 8 ] > > ( 7 - i % 8 ) ) & 1U ) ;
2017-06-12 17:22:52 +00:00
}
2021-09-05 22:05:29 +00:00
if ( d_enable_navdata_monitor )
{
d_nav_msg_packet . nav_message = raw_bits . to_string ( ) ;
}
2017-06-12 17:22:52 +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-06-12 17:22:52 +00:00
if ( d_CNAV_Message . have_new_ephemeris ( ) = = true )
{
// get ephemeris object for this SV
2020-07-19 07:39:32 +00:00
const std : : shared_ptr < Gps_CNAV_Ephemeris > tmp_obj = std : : make_shared < Gps_CNAV_Ephemeris > ( d_CNAV_Message . get_ephemeris ( ) ) ;
2020-07-07 16:53:50 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : ephemeris from satellite " < < d_satellite < < TEXT_RESET < < ' \n ' ;
2017-06-12 17:22:52 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
}
if ( d_CNAV_Message . have_new_iono ( ) = = true )
{
2020-07-19 07:39:32 +00:00
const std : : shared_ptr < Gps_CNAV_Iono > tmp_obj = std : : make_shared < Gps_CNAV_Iono > ( d_CNAV_Message . get_iono ( ) ) ;
2020-07-07 16:53:50 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : iono model parameters from satellite " < < d_satellite < < TEXT_RESET < < ' \n ' ;
2017-06-12 17:22:52 +00:00
this - > message_port_pub ( pmt : : mp ( " telemetry " ) , pmt : : make_any ( tmp_obj ) ) ;
}
if ( d_CNAV_Message . have_new_utc_model ( ) = = true )
{
2020-07-19 07:39:32 +00:00
const std : : shared_ptr < Gps_CNAV_Utc_Model > tmp_obj = std : : make_shared < Gps_CNAV_Utc_Model > ( d_CNAV_Message . get_utc_model ( ) ) ;
2020-07-07 16:53:50 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : UTC model parameters from satellite " < < d_satellite < < TEXT_RESET < < ' \n ' ;
2017-06-12 17:22:52 +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-02-22 18:16:06 +00:00
d_TOW_at_Preamble = static_cast < double > ( msg . tow ) ;
2019-03-18 15:28:49 +00:00
d_last_valid_preamble = d_sample_counter ;
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 * 20 + (12 * 20); 12 symbols of the encoder's transitory
2019-08-31 09:37:29 +00:00
d_TOW_at_current_symbol = static_cast < double > ( msg . tow ) * 6.0 + static_cast < double > ( delay ) * GPS_L2_M_PERIOD_S + 12 * GPS_L2_M_PERIOD_S ;
2019-08-18 20:16:13 +00:00
// d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
2017-06-12 17:22:52 +00:00
d_flag_valid_word = true ;
2021-09-05 22:05:29 +00:00
if ( d_enable_navdata_monitor & & ! d_nav_msg_packet . nav_message . empty ( ) )
{
d_nav_msg_packet . prn = static_cast < int32_t > ( current_synchro_data . PRN ) ;
d_nav_msg_packet . tow_at_current_symbol_ms = static_cast < int32_t > ( d_TOW_at_current_symbol * 1000.0 ) ;
const std : : shared_ptr < Nav_Message_Packet > tmp_obj = std : : make_shared < Nav_Message_Packet > ( d_nav_msg_packet ) ;
this - > message_port_pub ( pmt : : mp ( " Nav_msg_from_TLM " ) , pmt : : make_any ( tmp_obj ) ) ;
d_nav_msg_packet . nav_message = " " ;
}
2015-06-05 23:21:00 +00:00
}
else
{
2019-08-31 09:37:29 +00:00
d_TOW_at_current_symbol + = GPS_L2_M_PERIOD_S ;
2017-06-12 17:22:52 +00:00
if ( current_synchro_data . Flag_valid_symbol_output = = false )
{
d_flag_valid_word = false ;
}
2015-06-05 23:21:00 +00:00
}
2020-03-09 10:38:48 +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
2020-07-05 18:20:02 +00:00
current_synchro_data . Carrier_phase_rads + = GNSS_PI ;
2021-09-21 15:25:16 +00:00
current_synchro_data . Flag_PLL_180_deg_phase_locked = true ;
}
else
{
current_synchro_data . Flag_PLL_180_deg_phase_locked = false ;
2020-03-09 10:38:48 +00:00
}
2018-06-02 10:55:00 +00:00
current_synchro_data . TOW_at_current_symbol_ms = round ( d_TOW_at_current_symbol * 1000.0 ) ;
2017-06-12 17:22:52 +00:00
current_synchro_data . Flag_valid_word = d_flag_valid_word ;
2017-03-23 14:45:41 +00:00
2018-03-03 01:03:39 +00:00
if ( d_dump = = true )
2015-05-05 16:18:16 +00:00
{
2017-03-23 14:45:41 +00:00
// MULTIPLEXED FILE RECORDING - Record results to file
try
2018-03-03 01:03:39 +00:00
{
2017-06-12 17:22:52 +00:00
double tmp_double ;
2018-08-10 18:34:03 +00:00
uint64_t tmp_ulong_int ;
2020-11-19 08:55:08 +00:00
int32_t tmp_int ;
2017-06-12 17:22:52 +00:00
tmp_double = d_TOW_at_current_symbol ;
2018-03-03 01:03:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
2017-06-12 17:22:52 +00:00
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 ) ) ;
2017-06-12 17:22:52 +00:00
tmp_double = d_TOW_at_Preamble ;
2018-03-03 01:03:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
2020-11-19 08:55:08 +00:00
tmp_int = ( current_synchro_data . Prompt_I > 0.0 ? 1 : - 1 ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_int ) , sizeof ( int32_t ) ) ;
tmp_int = static_cast < int32_t > ( current_synchro_data . PRN ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_int ) , sizeof ( int32_t ) ) ;
2018-03-03 01:03:39 +00:00
}
catch ( const std : : ifstream : : failure & e )
{
2017-12-14 15:39:34 +00:00
LOG ( WARNING ) < < " Exception writing Telemetry GPS L2 dump file " < < e . what ( ) ;
2018-03-03 01:03:39 +00:00
}
2015-05-14 16:53:12 +00:00
}
2017-03-23 14:45:41 +00:00
2018-08-13 11:03:01 +00:00
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
2017-05-02 13:31:31 +00:00
out [ 0 ] = current_synchro_data ;
return 1 ;
2015-05-14 16:53:12 +00:00
}