2012-10-19 13:38:25 +00:00
/*!
* \ file galileo_e1_tcp_connector_tracking_cc . cc
* \ brief Implementation of a TCP connector block based on Code DLL + carrier PLL
* \ author David Pubill , 2012. dpubill ( at ) cttc . es
* Luis Esteve , 2012. luis ( at ) epsilon - formacion . com
* Javier Arribas , 2011. jarribas ( at ) cttc . es
*
*
* Code DLL + carrier PLL according to the algorithms described in :
* [ 1 ] K . Borre , D . M . Akos , N . Bertelsen , P . Rinder , and S . H . Jensen ,
* A Software - Defined GPS and Galileo Receiver . A Single - Frequency
* Approach , Birkha user , 2007
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2015-01-08 18:49:59 +00:00
* Copyright ( C ) 2010 - 2015 ( see AUTHORS file for a list of contributors )
2012-10-19 13:38:25 +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
2015-01-08 18:49:59 +00:00
* ( at your option ) any later version .
2012-10-19 13:38:25 +00:00
*
* 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
* along with GNSS - SDR . If not , see < http : //www.gnu.org/licenses/>.
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
# include "galileo_e1_tcp_connector_tracking_cc.h"
2014-03-16 19:58:29 +00:00
# include <cmath>
# include <iostream>
2014-07-19 17:29:27 +00:00
# include <memory>
2014-03-16 19:58:29 +00:00
# include <sstream>
# include <boost/asio.hpp>
# include <boost/lexical_cast.hpp>
# include <gnuradio/io_signature.h>
# include <glog/logging.h>
2016-08-18 12:17:02 +00:00
# include <volk_gnsssdr/volk_gnsssdr.h>
2012-10-19 13:38:25 +00:00
# include "galileo_e1_signal_processing.h"
# include "tracking_discriminators.h"
2012-10-21 09:54:37 +00:00
# include "lock_detectors.h"
2012-10-19 13:38:25 +00:00
# include "GPS_L1_CA.h"
# include "Galileo_E1.h"
# include "control_message_factory.h"
# include "tcp_communication.h"
# include "tcp_packet_data.h"
/*!
* \ todo Include in definition header file
*/
2012-10-20 16:11:31 +00:00
# define CN0_ESTIMATION_SAMPLES 20
2012-10-19 13:38:25 +00:00
# define MINIMUM_VALID_CN0 25
2012-10-20 16:11:31 +00:00
# define MAXIMUM_LOCK_FAIL_COUNTER 50
# define CARRIER_LOCK_THRESHOLD 0.85
2012-10-19 13:38:25 +00:00
using google : : LogMessage ;
2013-11-29 08:38:22 +00:00
galileo_e1_tcp_connector_tracking_cc_sptr galileo_e1_tcp_connector_make_tracking_cc (
2012-10-19 13:38:25 +00:00
long if_freq ,
long fs_in ,
unsigned int vector_length ,
bool dump ,
std : : string dump_filename ,
float pll_bw_hz ,
float dll_bw_hz ,
float early_late_space_chips ,
float very_early_late_space_chips ,
size_t port_ch0 )
{
return galileo_e1_tcp_connector_tracking_cc_sptr ( new Galileo_E1_Tcp_Connector_Tracking_cc ( if_freq ,
2016-05-09 09:51:42 +00:00
fs_in , vector_length , dump , dump_filename , pll_bw_hz , dll_bw_hz , early_late_space_chips , very_early_late_space_chips , port_ch0 ) ) ;
2012-10-19 13:38:25 +00:00
}
void Galileo_E1_Tcp_Connector_Tracking_cc : : forecast ( int noutput_items ,
gr_vector_int & ninput_items_required )
{
2015-12-02 17:25:06 +00:00
if ( noutput_items ! = 0 )
{
ninput_items_required [ 0 ] = ( int ) d_vector_length * 2 ; // set the required available samples in each call
}
2012-10-19 13:38:25 +00:00
}
2013-11-29 08:38:22 +00:00
2012-10-19 13:38:25 +00:00
Galileo_E1_Tcp_Connector_Tracking_cc : : Galileo_E1_Tcp_Connector_Tracking_cc (
long if_freq ,
long fs_in ,
unsigned int vector_length ,
bool dump ,
std : : string dump_filename ,
2016-04-06 13:04:52 +00:00
float pll_bw_hz __attribute__ ( ( unused ) ) ,
float dll_bw_hz __attribute__ ( ( unused ) ) ,
2012-10-19 13:38:25 +00:00
float early_late_space_chips ,
float very_early_late_space_chips ,
size_t port_ch0 ) :
2013-07-04 13:47:40 +00:00
gr : : block ( " Galileo_E1_Tcp_Connector_Tracking_cc " , gr : : io_signature : : make ( 1 , 1 , sizeof ( gr_complex ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2012-10-19 13:38:25 +00:00
{
2016-04-08 13:10:46 +00:00
// Telemetry bit synchronization message port input
this - > message_port_register_in ( pmt : : mp ( " preamble_timestamp_s " ) ) ;
2016-04-21 16:54:08 +00:00
this - > message_port_register_out ( pmt : : mp ( " events " ) ) ;
2012-10-19 13:38:25 +00:00
this - > set_relative_rate ( 1.0 / vector_length ) ;
// initialize internal vars
d_dump = dump ;
d_if_freq = if_freq ;
d_fs_in = fs_in ;
d_vector_length = vector_length ;
d_dump_filename = dump_filename ;
// Initialize tracking ==========================================
//--- DLL variables --------------------------------------------------------
2013-11-29 08:38:22 +00:00
d_early_late_spc_chips = early_late_space_chips ; // Define early-late offset (in chips)
2012-10-19 13:38:25 +00:00
d_very_early_late_spc_chips = very_early_late_space_chips ; // Define very-early-late offset (in chips)
2014-03-16 19:58:29 +00:00
//--- TCP CONNECTOR variables --------------------------------------------------------
d_port_ch0 = port_ch0 ;
d_port = 0 ;
d_listen_connection = true ;
d_control_id = 0 ;
2012-10-19 13:38:25 +00:00
// Initialization of local code replica
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
2016-08-18 12:17:02 +00:00
d_ca_code = static_cast < gr_complex * > ( volk_gnsssdr_malloc ( ( 2 * Galileo_E1_B_CODE_LENGTH_CHIPS ) * sizeof ( gr_complex ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2014-09-07 17:11:37 +00:00
2012-10-19 13:38:25 +00:00
// correlator outputs (scalar)
2016-04-06 12:58:18 +00:00
d_n_correlator_taps = 5 ; // Very-Early, Early, Prompt, Late, Very-Late
2016-08-18 12:17:02 +00:00
d_correlator_outs = static_cast < gr_complex * > ( volk_gnsssdr_malloc ( d_n_correlator_taps * sizeof ( gr_complex ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2016-04-06 12:58:18 +00:00
for ( int n = 0 ; n < d_n_correlator_taps ; n + + )
{
d_correlator_outs [ n ] = gr_complex ( 0 , 0 ) ;
}
// map memory pointers of correlator outputs
d_Very_Early = & d_correlator_outs [ 0 ] ;
d_Early = & d_correlator_outs [ 1 ] ;
d_Prompt = & d_correlator_outs [ 2 ] ;
d_Late = & d_correlator_outs [ 3 ] ;
d_Very_Late = & d_correlator_outs [ 4 ] ;
2016-08-18 12:17:02 +00:00
d_local_code_shift_chips = static_cast < float * > ( volk_gnsssdr_malloc ( d_n_correlator_taps * sizeof ( float ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2016-04-06 12:58:18 +00:00
// Set TAPs delay values [chips]
2016-08-11 11:51:14 +00:00
d_local_code_shift_chips [ 0 ] = - d_very_early_late_spc_chips ;
d_local_code_shift_chips [ 1 ] = - d_early_late_spc_chips ;
2016-04-06 12:58:18 +00:00
d_local_code_shift_chips [ 2 ] = 0.0 ;
2016-08-11 11:51:14 +00:00
d_local_code_shift_chips [ 3 ] = d_early_late_spc_chips ;
d_local_code_shift_chips [ 4 ] = d_very_early_late_spc_chips ;
2014-09-07 17:11:37 +00:00
2016-04-08 13:10:46 +00:00
d_correlation_length_samples = d_vector_length ;
2017-06-12 15:49:08 +00:00
2016-04-06 12:58:18 +00:00
multicorrelator_cpu . init ( 2 * d_correlation_length_samples , d_n_correlator_taps ) ;
2012-10-19 13:38:25 +00:00
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
2012-10-27 11:50:35 +00:00
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ ;
2012-10-19 13:38:25 +00:00
// define residual code phase (in chips)
d_rem_code_phase_samples = 0.0 ;
// define residual carrier phase
d_rem_carr_phase_rad = 0.0 ;
// sample synchronization
d_sample_counter = 0 ;
d_acq_sample_stamp = 0 ;
d_enable_tracking = false ;
d_pull_in = false ;
d_current_prn_length_samples = ( int ) d_vector_length ;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0 ;
d_Prompt_buffer = new gr_complex [ CN0_ESTIMATION_SAMPLES ] ;
d_carrier_lock_test = 1 ;
d_CN0_SNV_dB_Hz = 0 ;
d_carrier_lock_fail_counter = 0 ;
2012-10-20 16:11:31 +00:00
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD ;
2012-10-19 13:38:25 +00:00
systemName [ " E " ] = std : : string ( " Galileo " ) ;
2015-05-17 09:02:27 +00:00
d_acquisition_gnss_synchro = 0 ;
d_channel = 0 ;
d_next_rem_code_phase_samples = 0 ;
d_acq_code_phase_samples = 0.0 ;
d_acq_carrier_doppler_hz = 0.0 ;
d_acc_carrier_phase_rad = 0.0 ;
d_acc_code_phase_secs = 0.0 ;
d_code_phase_samples = 0 ;
d_next_prn_length_samples = 0 ;
2015-05-22 18:14:25 +00:00
d_carrier_doppler_hz = 0.0 ;
2012-10-19 13:38:25 +00:00
}
2013-11-29 08:38:22 +00:00
2012-10-19 13:38:25 +00:00
void Galileo_E1_Tcp_Connector_Tracking_cc : : start_tracking ( )
{
d_acq_code_phase_samples = d_acquisition_gnss_synchro - > Acq_delay_samples ;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro - > Acq_doppler_hz ;
d_acq_sample_stamp = d_acquisition_gnss_synchro - > Acq_samplestamp_samples ;
2016-04-06 12:58:18 +00:00
// generate local reference ALWAYS starting at chip 1 (2 samples per chip)
galileo_e1_code_gen_complex_sampled ( d_ca_code ,
2013-11-29 08:38:22 +00:00
d_acquisition_gnss_synchro - > Signal ,
false ,
d_acquisition_gnss_synchro - > PRN ,
2016-04-06 12:58:18 +00:00
2 * Galileo_E1_CODE_CHIP_RATE_HZ ,
2013-11-29 08:38:22 +00:00
0 ) ;
2016-04-06 12:58:18 +00:00
multicorrelator_cpu . set_local_code_and_taps ( static_cast < int > ( 2 * Galileo_E1_B_CODE_LENGTH_CHIPS ) , d_ca_code , d_local_code_shift_chips ) ;
for ( int n = 0 ; n < d_n_correlator_taps ; n + + )
{
d_correlator_outs [ n ] = gr_complex ( 0 , 0 ) ;
}
2012-10-27 11:50:35 +00:00
2012-10-19 13:38:25 +00:00
d_carrier_lock_fail_counter = 0 ;
d_rem_code_phase_samples = 0.0 ;
d_rem_carr_phase_rad = 0 ;
d_acc_carrier_phase_rad = 0 ;
2012-10-27 11:50:35 +00:00
d_acc_code_phase_secs = 0 ;
2012-10-19 13:38:25 +00:00
d_carrier_doppler_hz = d_acq_carrier_doppler_hz ;
2012-10-27 11:50:35 +00:00
d_current_prn_length_samples = d_vector_length ;
2012-10-19 13:38:25 +00:00
std : : string sys_ = & d_acquisition_gnss_synchro - > System ;
sys = sys_ . substr ( 0 , 1 ) ;
// DEBUG OUTPUT
2017-06-12 15:49:08 +00:00
std : : cout < < " Tracking of Galileo E1 signal started on channel " < < d_channel < < " for satellite " < < Gnss_Satellite ( systemName [ sys ] , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " Starting tracking of satellite " < < Gnss_Satellite ( systemName [ sys ] , d_acquisition_gnss_synchro - > PRN ) < < " on channel " < < d_channel ;
2012-10-19 13:38:25 +00:00
// enable tracking
d_pull_in = true ;
d_enable_tracking = true ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " PULL-IN Doppler [Hz]= " < < d_carrier_doppler_hz < < " PULL-IN Code Phase [samples]= " < < d_acq_code_phase_samples ;
2012-10-19 13:38:25 +00:00
}
Galileo_E1_Tcp_Connector_Tracking_cc : : ~ Galileo_E1_Tcp_Connector_Tracking_cc ( )
{
2017-07-25 14:50:22 +00:00
if ( d_dump_file . is_open ( ) )
{
try
{
d_dump_file . close ( ) ;
}
catch ( const std : : exception & ex )
{
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
}
}
try
{
volk_gnsssdr_free ( d_local_code_shift_chips ) ;
volk_gnsssdr_free ( d_correlator_outs ) ;
volk_gnsssdr_free ( d_ca_code ) ;
delete [ ] d_Prompt_buffer ;
d_tcp_com . close_tcp_connection ( d_port ) ;
multicorrelator_cpu . free ( ) ;
}
catch ( const std : : exception & ex )
{
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
}
2012-10-19 13:38:25 +00:00
}
2016-04-06 13:04:52 +00:00
int Galileo_E1_Tcp_Connector_Tracking_cc : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
2012-10-19 13:38:25 +00:00
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
{
// process vars
2012-10-27 11:50:35 +00:00
float carr_error_filt_hz ;
float code_error_filt_chips ;
2012-10-19 13:38:25 +00:00
tcp_packet_data tcp_data ;
2016-04-08 13:10:46 +00:00
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
2016-04-10 08:18:42 +00:00
Gnss_Synchro current_synchro_data = Gnss_Synchro ( ) ;
2016-04-08 13:10:46 +00:00
// Block input data and block output stream pointers
const gr_complex * in = ( gr_complex * ) input_items [ 0 ] ;
Gnss_Synchro * * out = ( Gnss_Synchro * * ) & output_items [ 0 ] ;
2012-10-19 13:38:25 +00:00
if ( d_enable_tracking = = true )
{
2016-04-08 13:10:46 +00:00
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2013-11-29 08:38:22 +00:00
if ( d_pull_in = = true )
{
/*
* Signal alignment ( skip samples until the incoming signal is aligned with local replica )
*/
int samples_offset ;
float acq_trk_shif_correction_samples ;
int acq_to_trk_delay_samples ;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp ;
2017-08-18 10:45:47 +00:00
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod ( static_cast < float > ( acq_to_trk_delay_samples ) , static_cast < float > ( d_current_prn_length_samples ) ) ;
2013-11-29 08:38:22 +00:00
samples_offset = round ( d_acq_code_phase_samples + acq_trk_shif_correction_samples ) ;
2017-04-12 15:04:51 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + samples_offset ;
2017-06-12 15:49:08 +00:00
current_synchro_data . fs = d_fs_in ;
2016-04-07 16:25:45 +00:00
* out [ 0 ] = current_synchro_data ;
2013-11-29 08:38:22 +00:00
d_sample_counter = d_sample_counter + samples_offset ; //count for the processed samples
d_pull_in = false ;
consume_each ( samples_offset ) ; //shift input to perform alignment with local replica
return 1 ;
}
2012-10-27 11:50:35 +00:00
2016-04-06 12:58:18 +00:00
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu . set_input_output_vectors ( d_correlator_outs , in ) ;
double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast < double > ( d_fs_in ) ;
double code_phase_step_half_chips = ( 2.0 * d_code_freq_chips ) / ( static_cast < double > ( d_fs_in ) ) ;
double rem_code_phase_half_chips = d_rem_code_phase_samples * ( 2.0 * d_code_freq_chips / d_fs_in ) ;
multicorrelator_cpu . Carrier_wipeoff_multicorrelator_resampler (
2016-05-02 21:46:30 +00:00
d_rem_carr_phase_rad ,
carr_phase_step_rad ,
rem_code_phase_half_chips ,
code_phase_step_half_chips ,
d_correlation_length_samples ) ;
2012-10-19 13:38:25 +00:00
2012-10-27 11:50:35 +00:00
// ################## TCP CONNECTOR ##########################################################
2012-10-19 13:38:25 +00:00
//! Variable used for control
d_control_id + + ;
//! Send and receive a TCP packet
2013-11-29 08:38:22 +00:00
boost : : array < float , NUM_TX_VARIABLES_GALILEO_E1 > tx_variables_array = { { d_control_id ,
( * d_Very_Early ) . real ( ) ,
( * d_Very_Early ) . imag ( ) ,
( * d_Early ) . real ( ) ,
( * d_Early ) . imag ( ) ,
( * d_Late ) . real ( ) ,
( * d_Late ) . imag ( ) ,
( * d_Very_Late ) . real ( ) ,
( * d_Very_Late ) . imag ( ) ,
( * d_Prompt ) . real ( ) ,
( * d_Prompt ) . imag ( ) ,
d_acq_carrier_doppler_hz ,
1 } } ;
2012-10-19 13:38:25 +00:00
d_tcp_com . send_receive_tcp_packet_galileo_e1 ( tx_variables_array , & tcp_data ) ;
2012-10-27 11:50:35 +00:00
// ################## PLL ##########################################################
2012-10-19 13:38:25 +00:00
// PLL discriminator, carrier loop filter implementation and NCO command generation (TCP_connector)
2012-10-27 11:50:35 +00:00
carr_error_filt_hz = tcp_data . proc_pack_carr_error ;
// New carrier Doppler frequency estimation
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz ;
// New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ( ( d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ ) / Galileo_E1_FREQ_HZ ) ;
//carrier phase accumulator for (K) doppler estimation
2015-11-26 17:44:04 +00:00
d_acc_carrier_phase_rad - = GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD ;
2013-11-29 08:38:22 +00:00
//remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD ;
d_rem_carr_phase_rad = fmod ( d_rem_carr_phase_rad , GPS_TWO_PI ) ;
2012-10-27 11:50:35 +00:00
// ################## DLL ##########################################################
2012-10-19 13:38:25 +00:00
// DLL discriminator, carrier loop filter implementation and NCO command generation (TCP_connector)
2012-10-27 11:50:35 +00:00
code_error_filt_chips = tcp_data . proc_pack_code_error ;
//Code phase accumulator
float code_error_filt_secs ;
2014-07-19 17:29:27 +00:00
code_error_filt_secs = ( Galileo_E1_CODE_PERIOD * code_error_filt_chips ) / Galileo_E1_CODE_CHIP_RATE_HZ ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs ;
2012-10-27 11:50:35 +00:00
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
2013-11-29 08:38:22 +00:00
// keep alignment parameters for the next input buffer
2014-07-19 17:29:27 +00:00
double T_chip_seconds ;
double T_prn_seconds ;
double T_prn_samples ;
double K_blk_samples ;
2013-11-29 08:38:22 +00:00
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation
2017-08-18 10:45:47 +00:00
T_chip_seconds = 1 / static_cast < double > ( d_code_freq_chips ) ;
2013-11-29 08:38:22 +00:00
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS ;
2017-08-18 10:45:47 +00:00
T_prn_samples = T_prn_seconds * static_cast < double > ( d_fs_in ) ;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast < double > ( d_fs_in ) ;
2013-11-29 08:38:22 +00:00
d_current_prn_length_samples = round ( K_blk_samples ) ; //round to a discrete samples
2014-08-31 11:21:41 +00:00
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
2013-11-29 08:38:22 +00:00
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if ( d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES )
{
// fill buffer with prompt correlator output values
d_Prompt_buffer [ d_cn0_estimation_counter ] = * d_Prompt ;
d_cn0_estimation_counter + + ;
}
else
{
d_cn0_estimation_counter = 0 ;
2012-10-19 13:38:25 +00:00
2013-11-29 08:38:22 +00:00
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator ( d_Prompt_buffer , CN0_ESTIMATION_SAMPLES , d_fs_in , Galileo_E1_B_CODE_LENGTH_CHIPS ) ;
2012-10-19 13:38:25 +00:00
2013-11-29 08:38:22 +00:00
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector ( d_Prompt_buffer , CN0_ESTIMATION_SAMPLES ) ;
2012-10-19 13:38:25 +00:00
2013-11-29 08:38:22 +00:00
// Loss of lock detection
if ( d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0 )
{
d_carrier_lock_fail_counter + + ;
}
else
{
if ( d_carrier_lock_fail_counter > 0 ) d_carrier_lock_fail_counter - - ;
}
if ( d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER )
{
2014-03-16 19:58:29 +00:00
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! " < < std : : endl ;
LOG ( INFO ) < < " Loss of lock in channel " < < d_channel < < " ! " ;
2016-04-21 16:54:08 +00:00
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; //3 -> loss of lock
2013-11-29 08:38:22 +00:00
d_carrier_lock_fail_counter = 0 ;
d_enable_tracking = false ; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// ########### Output the tracking data to navigation and PVT ##########
2017-08-18 10:45:47 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
2012-10-19 13:38:25 +00:00
// Tracking_timestamp_secs is aligned with the PRN start sample
2017-04-12 15:04:51 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples ;
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
2014-08-31 11:21:41 +00:00
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples ; //rounding error < 1 sample
2017-08-18 10:45:47 +00:00
current_synchro_data . Carrier_phase_rads = static_cast < double > ( d_acc_carrier_phase_rad ) ;
current_synchro_data . Carrier_Doppler_hz = static_cast < double > ( d_carrier_doppler_hz ) ;
current_synchro_data . CN0_dB_hz = static_cast < double > ( d_CN0_SNV_dB_Hz ) ;
2016-04-07 16:25:45 +00:00
current_synchro_data . Flag_valid_symbol_output = true ;
2016-04-08 13:10:46 +00:00
current_synchro_data . correlation_length_ms = 4 ;
2012-10-19 13:38:25 +00:00
}
else
{
* d_Early = gr_complex ( 0 , 0 ) ;
* d_Prompt = gr_complex ( 0 , 0 ) ;
* d_Late = gr_complex ( 0 , 0 ) ;
2017-04-12 15:04:51 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples ;
2012-10-19 13:38:25 +00:00
//! When tracking is disabled an array of 1's is sent to maintain the TCP connection
boost : : array < float , NUM_TX_VARIABLES_GALILEO_E1 > tx_variables_array = { { 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 0 } } ;
d_tcp_com . send_receive_tcp_packet_galileo_e1 ( tx_variables_array , & tcp_data ) ;
}
2016-04-07 16:25:45 +00:00
//assign the GNURadio block output data
2016-04-08 13:10:46 +00:00
current_synchro_data . System = { ' E ' } ;
2016-04-10 08:18:42 +00:00
std : : string str_aux = " 1B " ;
const char * str = str_aux . c_str ( ) ; // get a C style null terminated string
std : : memcpy ( ( void * ) current_synchro_data . Signal , str , 3 ) ;
2017-06-12 15:49:08 +00:00
current_synchro_data . fs = d_fs_in ;
2016-04-07 16:25:45 +00:00
* out [ 0 ] = current_synchro_data ;
2016-04-10 08:18:42 +00:00
2012-10-19 13:38:25 +00:00
if ( d_dump )
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I ;
float prompt_Q ;
float tmp_VE , tmp_E , tmp_P , tmp_L , tmp_VL ;
float tmp_float ;
2016-04-08 13:10:46 +00:00
tmp_float = 0 ;
2012-10-19 13:38:25 +00:00
double tmp_double ;
prompt_I = ( * d_Prompt ) . real ( ) ;
prompt_Q = ( * d_Prompt ) . imag ( ) ;
tmp_VE = std : : abs < float > ( * d_Very_Early ) ;
tmp_E = std : : abs < float > ( * d_Early ) ;
tmp_P = std : : abs < float > ( * d_Prompt ) ;
tmp_L = std : : abs < float > ( * d_Late ) ;
tmp_VL = std : : abs < float > ( * d_Very_Late ) ;
try
{
// EPR
d_dump_file . write ( ( char * ) & tmp_VE , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & tmp_E , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & tmp_P , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & tmp_L , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & tmp_VL , sizeof ( float ) ) ;
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file . write ( ( char * ) & prompt_I , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & prompt_Q , sizeof ( float ) ) ;
// PRN start sample stamp
d_dump_file . write ( ( char * ) & d_sample_counter , sizeof ( unsigned long int ) ) ;
// accumulated carrier phase
d_dump_file . write ( ( char * ) & d_acc_carrier_phase_rad , sizeof ( float ) ) ;
// carrier and code frequency
d_dump_file . write ( ( char * ) & d_carrier_doppler_hz , sizeof ( float ) ) ;
2012-10-27 11:50:35 +00:00
d_dump_file . write ( ( char * ) & d_code_freq_chips , sizeof ( float ) ) ;
2012-10-19 13:38:25 +00:00
//PLL commands
d_dump_file . write ( ( char * ) & tmp_float , sizeof ( float ) ) ;
2012-10-27 11:50:35 +00:00
d_dump_file . write ( ( char * ) & carr_error_filt_hz , sizeof ( float ) ) ;
2012-10-19 13:38:25 +00:00
//DLL commands
d_dump_file . write ( ( char * ) & tmp_float , sizeof ( float ) ) ;
2012-10-27 11:50:35 +00:00
d_dump_file . write ( ( char * ) & code_error_filt_chips , sizeof ( float ) ) ;
2012-10-19 13:38:25 +00:00
// CN0 and carrier lock test
d_dump_file . write ( ( char * ) & d_CN0_SNV_dB_Hz , sizeof ( float ) ) ;
d_dump_file . write ( ( char * ) & d_carrier_lock_test , sizeof ( float ) ) ;
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples ;
d_dump_file . write ( ( char * ) & tmp_float , sizeof ( float ) ) ;
2017-08-18 10:45:47 +00:00
tmp_double = static_cast < double > ( d_sample_counter + d_current_prn_length_samples ) ;
2012-10-19 13:38:25 +00:00
d_dump_file . write ( ( char * ) & tmp_double , sizeof ( double ) ) ;
2017-06-16 19:24:14 +00:00
// PRN
unsigned int prn_ = d_acquisition_gnss_synchro - > PRN ;
d_dump_file . write ( reinterpret_cast < char * > ( & prn_ ) , sizeof ( unsigned int ) ) ;
2012-10-19 13:38:25 +00:00
}
2016-04-06 12:58:18 +00:00
catch ( const std : : ifstream : : failure & e )
2012-10-19 13:38:25 +00:00
{
2014-03-16 19:58:29 +00:00
LOG ( WARNING ) < < " Exception writing trk dump file " < < e . what ( ) ;
2012-10-19 13:38:25 +00:00
}
}
2013-11-29 08:38:22 +00:00
consume_each ( d_current_prn_length_samples ) ; // this is needed in gr::block derivates
2012-10-19 13:38:25 +00:00
d_sample_counter + = d_current_prn_length_samples ; //count for the processed samples
2016-04-06 13:04:52 +00:00
2012-10-19 13:38:25 +00:00
return 1 ; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
void Galileo_E1_Tcp_Connector_Tracking_cc : : set_channel ( unsigned int channel )
{
d_channel = channel ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " Tracking Channel set to " < < d_channel ;
2012-10-19 13:38:25 +00:00
// ############# ENABLE DATA FILE LOG #################
2014-03-16 19:58:29 +00:00
if ( d_dump = = true )
2012-10-19 13:38:25 +00:00
{
if ( d_dump_file . is_open ( ) = = false )
{
try
{
d_dump_filename . append ( boost : : lexical_cast < std : : string > ( d_channel ) ) ;
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 ) ;
2014-03-16 19:58:29 +00:00
LOG ( INFO ) < < " Tracking dump enabled on channel " < < d_channel < < " Log file: " < < d_dump_filename . c_str ( ) ;
2012-10-19 13:38:25 +00:00
}
2016-04-06 12:58:18 +00:00
catch ( const std : : ifstream : : failure & e )
2012-10-19 13:38:25 +00:00
{
2014-03-16 19:58:29 +00:00
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening trk dump file " < < e . what ( ) ;
2012-10-19 13:38:25 +00:00
}
}
}
2013-11-29 08:38:22 +00:00
//! Listen for connections on a TCP port
2012-10-19 13:38:25 +00:00
if ( d_listen_connection = = true )
2013-11-29 08:38:22 +00:00
{
d_port = d_port_ch0 + d_channel ;
d_listen_connection = d_tcp_com . listen_tcp_connection ( d_port , d_port_ch0 ) ;
}
2012-10-19 13:38:25 +00:00
}
void Galileo_E1_Tcp_Connector_Tracking_cc : : set_gnss_synchro ( Gnss_Synchro * p_gnss_synchro )
{
d_acquisition_gnss_synchro = p_gnss_synchro ;
}