2015-05-05 16:18:16 +00:00
/*!
2019-03-02 01:21:03 +00:00
* \ file gps_l2c_telemetry_decoder_gs . cc
2015-05-05 16:18:16 +00:00
* \ brief Implementation of a NAV message demodulator block based on
* Kay Borre book MATLAB - based GPS receiver
* \ author Javier Arribas , 2015. jarribas ( at ) cttc . es
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2018-05-13 20:49:11 +00:00
* Copyright ( C ) 2010 - 2018 ( see AUTHORS file for a list of contributors )
2015-05-05 16:18:16 +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/>.
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"
2018-02-26 11:33:40 +00:00
# include "display.h"
2018-12-09 21:00:09 +00:00
# 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>
2017-03-23 14:45:41 +00:00
# include <bitset>
2015-05-05 16:18:16 +00:00
# include <iostream>
# include <sstream>
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
gps_l2c_make_telemetry_decoder_gs ( const Gnss_Satellite & satellite , bool dump )
2015-05-05 16:18:16 +00:00
{
2019-03-02 01:21:03 +00:00
return gps_l2c_telemetry_decoder_gs_sptr ( new gps_l2c_telemetry_decoder_gs ( satellite , dump ) ) ;
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 (
const Gnss_Satellite & satellite , bool dump ) : gr : : block ( " gps_l2c_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 ) ) )
2015-05-05 16:18:16 +00:00
{
2016-04-12 15:29:16 +00:00
// Ephemeris data port out
this - > message_port_register_out ( pmt : : mp ( " telemetry " ) ) ;
2015-05-05 16:18:16 +00:00
// initialize internal vars
d_dump = dump ;
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
// set_output_multiple (1);
2015-06-05 23:21:00 +00:00
d_channel = 0 ;
d_flag_valid_word = false ;
d_TOW_at_current_symbol = 0 ;
d_TOW_at_Preamble = 0 ;
2018-03-03 01:03:39 +00:00
d_state = 0 ; //initial state
2017-06-12 17:22:52 +00:00
d_crc_error_count = 0 ;
2017-03-23 14:45:41 +00:00
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 ) ;
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
{
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
}
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
{
d_dump_filename = " telemetry_L2CM_ " ;
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 ( ) ;
}
}
}
}
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
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 ) ;
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
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
{
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
{
2018-03-03 01:03:39 +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
}
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
std : : shared_ptr < Gps_CNAV_Ephemeris > tmp_obj = std : : make_shared < Gps_CNAV_Ephemeris > ( d_CNAV_Message . get_ephemeris ( ) ) ;
2018-04-22 20:53:33 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : ephemeris from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
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 )
{
std : : shared_ptr < Gps_CNAV_Iono > tmp_obj = std : : make_shared < Gps_CNAV_Iono > ( d_CNAV_Message . get_iono ( ) ) ;
2018-04-22 20:53:33 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : iono model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
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 )
{
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 20:53:33 +00:00
std : : cout < < TEXT_BLUE < < " New GPS CNAV message received in channel " < < d_channel < < " : UTC model parameters from satellite " < < d_satellite < < TEXT_RESET < < std : : endl ;
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 ) ;
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
2017-08-18 10:45:47 +00:00
d_TOW_at_current_symbol = static_cast < double > ( msg . tow ) * 6.0 + static_cast < double > ( delay ) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD ;
2018-02-22 18:16:06 +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 ;
2015-06-05 23:21:00 +00:00
}
else
{
2017-06-12 17:22:52 +00:00
d_TOW_at_current_symbol + = GPS_L2_M_PERIOD ;
if ( current_synchro_data . Flag_valid_symbol_output = = false )
{
d_flag_valid_word = false ;
}
2015-06-05 23:21:00 +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 ;
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 ) ) ;
}
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
}