2017-11-29 14:51:30 +00:00
/*!
* \ file gps_l5_telemetry_decoder_cc . 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
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2018-05-13 20:49:11 +00:00
* Copyright ( C ) 2010 - 2018 ( 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 .
*
* 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-05-13 20:49:11 +00:00
* along with GNSS - SDR . If not , see < https : //www.gnu.org/licenses/>.
2017-11-29 14:51:30 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
2018-02-26 02:15:53 +00:00
2017-11-29 14:51:30 +00:00
# include "gps_l5_telemetry_decoder_cc.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"
2018-02-26 02:15:53 +00:00
# include <boost/lexical_cast.hpp>
2018-04-22 21:07:34 +00:00
# include <glog/logging.h>
# include <gnuradio/io_signature.h>
2018-02-26 02:15:53 +00:00
# include <bitset>
# include <iostream>
# include <sstream>
2017-11-29 14:51:30 +00:00
using google : : LogMessage ;
gps_l5_telemetry_decoder_cc_sptr
2018-03-03 01:03:39 +00:00
gps_l5_make_telemetry_decoder_cc ( const Gnss_Satellite & satellite , bool dump )
2017-11-29 14:51:30 +00:00
{
return gps_l5_telemetry_decoder_cc_sptr ( new gps_l5_telemetry_decoder_cc ( satellite , dump ) ) ;
}
gps_l5_telemetry_decoder_cc : : gps_l5_telemetry_decoder_cc (
2018-03-03 01:03:39 +00:00
const Gnss_Satellite & satellite , bool dump ) : gr : : block ( " gps_l5_telemetry_decoder_cc " ,
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
{
// Ephemeris data port out
this - > message_port_register_out ( pmt : : mp ( " telemetry " ) ) ;
// 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-02-22 09:47:24 +00:00
for ( int32_t aux = 0 ; aux < GPS_L5I_NH_CODE_LENGTH ; aux + + )
2017-12-11 11:17:01 +00:00
{
2019-02-22 09:47:24 +00:00
if ( GPS_L5I_NH_CODE [ aux ] = = 0 )
2017-12-11 11:17:01 +00:00
{
bits_NH [ aux ] = - 1.0 ;
}
else
{
bits_NH [ aux ] = 1.0 ;
}
}
sync_NH = false ;
new_sym = false ;
2017-11-29 14:51:30 +00:00
}
gps_l5_telemetry_decoder_cc : : ~ gps_l5_telemetry_decoder_cc ( )
{
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
}
}
2018-04-13 09:07:58 +00:00
void gps_l5_telemetry_decoder_cc : : set_satellite ( const Gnss_Satellite & satellite )
{
d_satellite = Gnss_Satellite ( satellite . get_system ( ) , satellite . get_PRN ( ) ) ;
LOG ( INFO ) < < " GPS L5 CNAV telemetry decoder in channel " < < this - > d_channel < < " set to satellite " < < d_satellite ;
d_CNAV_Message . reset ( ) ;
}
2018-08-13 11:03:01 +00:00
void gps_l5_telemetry_decoder_cc : : set_channel ( int32_t channel )
2018-04-13 09:07:58 +00:00
{
d_channel = channel ;
d_CNAV_Message . reset ( ) ;
LOG ( INFO ) < < " GPS L5 CNAV channel set to " < < channel ;
// ############# 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 ( ) ;
}
}
}
}
2018-03-03 01:03:39 +00:00
int gps_l5_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 )
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
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
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 ] ;
2018-03-03 01:03:39 +00:00
consume_each ( 1 ) ; //one by one
2017-12-11 11:17:01 +00:00
sym_hist . push_back ( in [ 0 ] . Prompt_I ) ;
2018-08-13 11:03:01 +00:00
int32_t corr_NH = 0 ;
int32_t symbol_value = 0 ;
2017-11-29 14:51:30 +00:00
2018-08-13 11:03:01 +00:00
// Search correlation with Neuman-Hofman Code (see IS-GPS-705D)
2019-02-22 09:47:24 +00:00
if ( sym_hist . size ( ) = = GPS_L5I_NH_CODE_LENGTH )
2017-12-11 11:17:01 +00:00
{
2019-02-22 09:47:24 +00:00
for ( int32_t i = 0 ; i < GPS_L5I_NH_CODE_LENGTH ; i + + )
2017-12-11 11:17:01 +00:00
{
2018-03-03 01:03:39 +00:00
if ( ( bits_NH [ i ] * sym_hist . at ( i ) ) > 0.0 )
{
corr_NH + = 1 ;
}
else
{
corr_NH - = 1 ;
}
2017-12-14 15:39:34 +00:00
}
2019-02-22 09:47:24 +00:00
if ( abs ( corr_NH ) = = GPS_L5I_NH_CODE_LENGTH )
2017-12-14 15:39:34 +00:00
{
sync_NH = true ;
2018-03-03 01:03:39 +00:00
if ( corr_NH > 0 )
{
symbol_value = 1 ;
}
else
{
symbol_value = - 1 ;
}
2017-12-14 15:39:34 +00:00
new_sym = true ;
sym_hist . clear ( ) ;
2017-12-11 11:17:01 +00:00
}
else
{
sym_hist . pop_front ( ) ;
sync_NH = false ;
2017-12-14 15:39:34 +00:00
new_sym = false ;
2017-12-11 11:17:01 +00:00
}
}
bool flag_new_cnav_frame = false ;
cnav_msg_t msg ;
2018-08-13 11:03:01 +00:00
uint32_t delay = 0 ;
2017-12-11 11:17:01 +00:00
2018-08-13 11:03:01 +00:00
// add the symbol to the decoder
2018-03-03 01:03:39 +00:00
if ( new_sym )
2017-12-11 11:17:01 +00:00
{
2018-08-13 11:03:01 +00:00
uint8_t symbol_clip = static_cast < uint8_t > ( symbol_value > 0 ) * 255 ;
2017-12-11 11:17:01 +00:00
flag_new_cnav_frame = cnav_msg_decoder_add_symbol ( & d_cnav_decoder , symbol_clip , & msg , & delay ) ;
new_sym = false ;
}
2018-08-13 11:03:01 +00:00
// 2. Add the telemetry decoder information
// check if new CNAV frame is available
2017-11-29 14:51:30 +00:00
if ( flag_new_cnav_frame = = true )
{
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
{
2018-03-03 01:03:39 +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 ;
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-02-22 09:47:24 +00:00
d_TOW_at_current_symbol_ms = msg . tow * 6000 + ( delay + 12 ) * GPS_L5I_SYMBOL_PERIOD_MS ;
2017-11-29 14:51:30 +00:00
d_flag_valid_word = true ;
}
else
{
2019-02-22 09:47:24 +00:00
d_TOW_at_current_symbol_ms + = GPS_L5I_PERIOD_MS ;
2017-11-29 14:51:30 +00:00
if ( current_synchro_data . Flag_valid_symbol_output = = false )
{
d_flag_valid_word = false ;
}
}
2018-08-07 17:39:56 +00:00
if ( d_flag_valid_word = = true )
2017-11-29 14:51:30 +00:00
{
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
}