2018-03-02 16:40:13 +00:00
/*!
2018-05-22 10:25:14 +00:00
* \ file dll_pll_veml_tracking_fpga . cc
2018-05-09 09:11:12 +00:00
* \ brief Implementation of a code DLL + carrier PLL tracking block using an FPGA
2019-02-27 12:30:09 +00:00
* \ author Marc Majoral , 2019. marc . majoral ( at ) cttc . es
2019-03-01 09:11:36 +00:00
* \ author Javier Arribas , 2019. jarribas ( at ) cttc . es
2018-03-02 16:40:13 +00:00
*
* 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 , Birkhauser , 2007
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2018-03-02 16:40:13 +00:00
*
2020-12-30 12:35:06 +00:00
* GNSS - SDR is a Global Navigation Satellite System software - defined receiver .
2018-03-02 16:40:13 +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
2018-03-02 16:40:13 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2018-03-02 16:40:13 +00:00
*/
2020-12-30 12:35:06 +00:00
2018-05-03 08:38:21 +00:00
# include "dll_pll_veml_tracking_fpga.h"
2019-02-27 16:27:31 +00:00
# include "GPS_L1_CA.h"
# include "GPS_L2C.h"
# include "GPS_L5.h"
2018-05-09 09:11:12 +00:00
# include "Galileo_E1.h"
# include "Galileo_E5a.h"
2019-02-27 16:27:31 +00:00
# include "MATH_CONSTANTS.h"
2019-03-06 13:36:51 +00:00
# include "fpga_multicorrelator.h"
2020-04-25 14:08:33 +00:00
# include "gnss_satellite.h"
2019-02-27 16:27:31 +00:00
# include "gnss_sdr_create_directory.h"
2021-01-16 13:37:19 +00:00
# include "gnss_sdr_filesystem.h"
2019-03-06 13:36:51 +00:00
# include "gnss_synchro.h"
2020-11-07 21:43:19 +00:00
# include "gps_sdr_signal_replica.h"
2019-02-27 16:27:31 +00:00
# include "lock_detectors.h"
# include "tracking_discriminators.h"
2018-05-09 09:11:12 +00:00
# include <glog/logging.h>
2019-07-17 16:01:27 +00:00
# include <gnuradio/io_signature.h> // for io_signature
# include <gnuradio/thread/thread.h> // for scoped_lock
2020-05-07 19:47:45 +00:00
# include <matio.h> // for Mat_VarCreate
# include <pmt/pmt_sugar.h> // for mp
2018-05-09 09:11:12 +00:00
# include <volk_gnsssdr/volk_gnsssdr.h>
2019-07-17 16:01:27 +00:00
# include <algorithm> // for fill_n
2019-09-19 16:07:13 +00:00
# include <cmath> // for fmod, round, floor
2019-07-17 16:01:27 +00:00
# include <exception> // for exception
2019-07-29 19:47:46 +00:00
# include <iostream> // for cout, cerr
2019-03-06 13:36:51 +00:00
# include <map>
2019-07-09 17:15:27 +00:00
# include <numeric>
2019-06-30 12:32:40 +00:00
# include <vector>
2018-08-01 13:55:40 +00:00
2020-04-25 20:17:15 +00:00
# if HAS_GENERIC_LAMBDA
# else
2020-06-06 21:41:38 +00:00
# include <boost/bind/bind.hpp>
2020-04-25 20:17:15 +00:00
# endif
2021-10-30 03:43:22 +00:00
# if PMT_USES_BOOST_ANY
# include <boost/any.hpp>
namespace wht = boost ;
# else
# include <any>
namespace wht = std ;
# endif
2019-06-10 19:41:13 +00:00
2018-08-02 15:32:59 +00:00
dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga ( const Dll_Pll_Conf_Fpga & conf_ )
2018-03-02 16:40:13 +00:00
{
2018-05-03 08:38:21 +00:00
return dll_pll_veml_tracking_fpga_sptr ( new dll_pll_veml_tracking_fpga ( conf_ ) ) ;
2018-03-02 16:40:13 +00:00
}
2018-05-03 08:38:21 +00:00
2021-10-11 13:13:12 +00:00
dll_pll_veml_tracking_fpga : : dll_pll_veml_tracking_fpga ( const Dll_Pll_Conf_Fpga & conf_ )
: gr : : block ( " dll_pll_veml_tracking_fpga " , gr : : io_signature : : make ( 0 , 0 , sizeof ( lv_16sc_t ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ) ,
d_trk_parameters ( conf_ ) ,
d_acquisition_gnss_synchro ( nullptr ) ,
d_code_chip_rate ( 0.0 ) ,
d_code_phase_step_chips ( 0.0 ) ,
d_code_phase_rate_step_chips ( 0.0 ) ,
d_carrier_phase_step_rad ( 0.0 ) ,
d_carrier_phase_rate_step_rad ( 0.0 ) ,
d_acq_code_phase_samples ( 0.0 ) ,
d_acq_carrier_doppler_hz ( 0.0 ) ,
d_rem_code_phase_samples ( 0.0 ) ,
d_rem_code_phase_samples_prev ( 0.0 ) ,
d_current_correlation_time_s ( 0.0 ) ,
d_carrier_doppler_hz ( 0.0 ) ,
d_acc_carrier_phase_rad ( 0.0 ) ,
d_rem_code_phase_chips ( 0.0 ) ,
d_T_chip_seconds ( 0.0 ) ,
d_T_prn_seconds ( 0.0 ) ,
d_T_prn_samples ( 0.0 ) ,
d_K_blk_samples ( 0.0 ) ,
d_carrier_lock_test ( 1.0 ) ,
d_CN0_SNV_dB_Hz ( 0.0 ) ,
d_carrier_lock_threshold ( d_trk_parameters . carrier_lock_th ) ,
d_sample_counter ( 0ULL ) ,
d_acq_sample_stamp ( 0ULL ) ,
d_sample_counter_next ( 0ULL ) ,
d_rem_carr_phase_rad ( 0.0 ) ,
d_state ( 1 ) ,
d_extend_correlation_symbols_count ( 0 ) ,
d_current_integration_length_samples ( static_cast < int32_t > ( d_trk_parameters . vector_length ) ) ,
d_cn0_estimation_counter ( 0 ) ,
d_carrier_lock_fail_counter ( 0 ) ,
d_code_lock_fail_counter ( 0 ) ,
d_extend_fpga_integration_periods ( d_trk_parameters . extend_fpga_integration_periods ) ,
d_channel ( 0 ) ,
d_secondary_code_length ( 0U ) ,
d_data_secondary_code_length ( 0U ) ,
d_code_length_chips ( d_trk_parameters . code_length_chips ) ,
d_code_samples_per_chip ( d_trk_parameters . code_samples_per_chip ) ,
d_fpga_integration_period ( d_trk_parameters . fpga_integration_period ) ,
d_current_fpga_integration_period ( 1 ) ,
d_veml ( false ) ,
d_cloop ( true ) ,
d_dump ( d_trk_parameters . dump ) ,
d_dump_mat ( d_trk_parameters . dump_mat & & d_dump ) ,
d_pull_in_transitory ( true ) ,
d_corrected_doppler ( false ) ,
d_interchange_iq ( false ) ,
d_acc_carrier_phase_initialized ( false ) ,
d_worker_is_done ( false ) ,
d_extended_correlation_in_fpga ( d_trk_parameters . extended_correlation_in_fpga ) ,
d_current_extended_correlation_in_fpga ( false ) ,
d_stop_tracking ( false ) ,
d_sc_demodulate_enabled ( false ) ,
d_Flag_PLL_180_deg_phase_locked ( false )
2018-03-02 16:40:13 +00:00
{
2022-12-03 23:11:42 +00:00
# if GNURADIO_GREATER_THAN_38
this - > set_relative_rate ( 1 , static_cast < uint64_t > ( d_trk_parameters . vector_length ) ) ;
# else
this - > set_relative_rate ( 1.0 / static_cast < double > ( d_trk_parameters . vector_length ) ) ;
# endif
2019-07-28 10:01:11 +00:00
// prevent telemetry symbols accumulation in output buffers
2019-07-17 16:01:27 +00:00
this - > set_max_noutput_items ( 1 ) ;
2019-07-28 10:01:11 +00:00
2018-03-02 16:40:13 +00:00
// Telemetry bit synchronization message port input
this - > message_port_register_out ( pmt : : mp ( " events " ) ) ;
2018-05-03 08:38:21 +00:00
2019-03-18 20:35:55 +00:00
// Telemetry message port input
this - > message_port_register_in ( pmt : : mp ( " telemetry_to_trk " ) ) ;
2020-04-25 20:17:15 +00:00
this - > set_msg_handler ( pmt : : mp ( " telemetry_to_trk " ) ,
# if HAS_GENERIC_LAMBDA
2020-06-08 18:04:01 +00:00
[ this ] ( auto & & PH1 ) { msg_handler_telemetry_to_trk ( PH1 ) ; } ) ;
2020-04-25 20:17:15 +00:00
# else
2020-06-12 22:32:40 +00:00
# if USE_BOOST_BIND_PLACEHOLDERS
2020-05-18 16:58:39 +00:00
boost : : bind ( & dll_pll_veml_tracking_fpga : : msg_handler_telemetry_to_trk , this , boost : : placeholders : : _1 ) ) ;
2020-05-18 20:50:34 +00:00
# else
boost : : bind ( & dll_pll_veml_tracking_fpga : : msg_handler_telemetry_to_trk , this , _1 ) ) ;
# endif
2020-04-25 20:17:15 +00:00
# endif
2018-03-02 16:40:13 +00:00
// initialize internal vars
2019-07-09 18:11:21 +00:00
d_dll_filt_history . set_capacity ( 1000 ) ;
2021-10-11 13:13:12 +00:00
2020-06-19 01:22:01 +00:00
d_signal_type = std : : string ( d_trk_parameters . signal ) ;
2018-05-09 09:11:12 +00:00
std : : map < std : : string , std : : string > map_signal_pretty_name ;
map_signal_pretty_name [ " 1C " ] = " L1 C/A " ;
map_signal_pretty_name [ " 1B " ] = " E1 " ;
map_signal_pretty_name [ " 1G " ] = " L1 C/A " ;
map_signal_pretty_name [ " 2S " ] = " L2C " ;
map_signal_pretty_name [ " 2G " ] = " L2 C/A " ;
map_signal_pretty_name [ " 5X " ] = " E5a " ;
map_signal_pretty_name [ " L5 " ] = " L5 " ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = map_signal_pretty_name [ d_signal_type ] ;
2018-05-09 09:11:12 +00:00
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . system = = ' G ' )
2018-05-09 09:11:12 +00:00
{
2020-06-19 01:22:01 +00:00
d_systemName = " GPS " ;
if ( d_signal_type = = " 1C " )
2018-05-09 09:11:12 +00:00
{
d_signal_carrier_freq = GPS_L1_FREQ_HZ ;
2019-08-31 09:37:29 +00:00
d_code_period = GPS_L1_CA_CODE_PERIOD_S ;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_CPS ;
2018-05-09 09:11:12 +00:00
d_correlation_length_ms = 1 ;
// GPS L1 C/A does not have pilot component nor secondary code
d_secondary = false ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . track_pilot = false ;
d_trk_parameters . slope = 1.0 ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
d_trk_parameters . y_intercept = 1.0 ;
2019-07-10 16:02:57 +00:00
// symbol integration: 20 trk symbols (20 ms) = 1 tlm bit
2021-09-27 15:44:44 +00:00
// set the bit transition pattern in secondary code to obtain bit synchronization
2021-10-05 19:38:32 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GPS_CA_PREAMBLE_LENGTH_SYMBOLS ) ;
d_secondary_code_string = GPS_CA_PREAMBLE_SYMBOLS_STR ;
2019-07-17 16:01:27 +00:00
d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT ;
2018-05-09 09:11:12 +00:00
}
2020-06-19 01:22:01 +00:00
else if ( d_signal_type = = " 2S " )
2018-05-09 09:11:12 +00:00
{
d_signal_carrier_freq = GPS_L2_FREQ_HZ ;
2019-08-31 09:37:29 +00:00
d_code_period = GPS_L2_M_PERIOD_S ;
d_code_chip_rate = GPS_L2_M_CODE_RATE_CPS ;
2019-07-17 16:01:27 +00:00
// GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required
2018-05-09 09:11:12 +00:00
d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL ;
d_correlation_length_ms = 20 ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . slope = 1.0 ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
d_trk_parameters . y_intercept = 1.0 ;
2018-05-09 09:11:12 +00:00
// GPS L2 does not have pilot component nor secondary code
d_secondary = false ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . track_pilot = false ;
2018-05-09 09:11:12 +00:00
}
2020-06-19 01:22:01 +00:00
else if ( d_signal_type = = " L5 " )
2018-05-09 09:11:12 +00:00
{
d_signal_carrier_freq = GPS_L5_FREQ_HZ ;
2019-08-31 09:37:29 +00:00
d_code_period = GPS_L5I_PERIOD_S ;
d_code_chip_rate = GPS_L5I_CODE_RATE_CPS ;
2019-07-10 16:02:57 +00:00
// symbol integration: 10 trk symbols (10 ms) = 1 tlm bit
2018-05-09 09:11:12 +00:00
d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL ;
d_correlation_length_ms = 1 ;
d_secondary = true ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . slope = 1.0 ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
d_trk_parameters . y_intercept = 1.0 ;
2019-07-19 09:55:53 +00:00
if ( d_extended_correlation_in_fpga = = true )
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . extend_correlation_symbols > 1 )
2019-07-19 09:55:53 +00:00
{
d_sc_demodulate_enabled = true ;
}
}
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2018-05-09 09:11:12 +00:00
{
2019-07-17 16:01:27 +00:00
// synchronize pilot secondary code
2019-02-22 14:57:15 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GPS_L5Q_NH_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_secondary_code_string = GPS_L5Q_NH_CODE_STR ;
2019-07-10 16:02:57 +00:00
// remove data secondary code
// remove Neuman-Hofman Code (see IS-GPS-705D)
d_data_secondary_code_length = static_cast < uint32_t > ( GPS_L5I_NH_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_data_secondary_code_string = GPS_L5I_NH_CODE_STR ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " Q " ;
2018-05-09 09:11:12 +00:00
}
else
{
2019-07-17 16:01:27 +00:00
// synchronize and remove data secondary code
// remove Neuman-Hofman Code (see IS-GPS-705D)
2019-02-22 14:57:15 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GPS_L5I_NH_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_secondary_code_string = GPS_L5I_NH_CODE_STR ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " I " ;
d_interchange_iq = true ;
2018-05-09 09:11:12 +00:00
}
}
else
{
LOG ( WARNING ) < < " Invalid Signal argument when instantiating tracking blocks " ;
2020-07-07 16:53:50 +00:00
std : : cerr < < " Invalid Signal argument when instantiating tracking blocks \n " ;
2018-05-09 09:11:12 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
d_symbols_per_bit = 0 ;
}
}
2020-06-19 01:22:01 +00:00
else if ( d_trk_parameters . system = = ' E ' )
2018-05-09 09:11:12 +00:00
{
2020-06-19 01:22:01 +00:00
d_systemName = " Galileo " ;
if ( d_signal_type = = " 1B " )
2018-05-09 09:11:12 +00:00
{
2019-02-22 14:57:15 +00:00
d_signal_carrier_freq = GALILEO_E1_FREQ_HZ ;
2019-08-31 09:37:29 +00:00
d_code_period = GALILEO_E1_CODE_PERIOD_S ;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_CPS ;
2019-07-10 16:02:57 +00:00
// Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required
2018-05-09 09:11:12 +00:00
d_symbols_per_bit = 1 ;
d_correlation_length_ms = 4 ;
d_veml = true ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
2020-07-10 19:13:17 +00:00
d_trk_parameters . slope = static_cast < float > ( - CalculateSlopeAbs ( & SinBocCorrelationFunction < 1 , 1 > , d_trk_parameters . spc ) ) ;
d_trk_parameters . y_intercept = static_cast < float > ( GetYInterceptAbs ( & SinBocCorrelationFunction < 1 , 1 > , d_trk_parameters . spc ) ) ;
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2018-05-09 09:11:12 +00:00
{
d_secondary = true ;
2019-02-22 14:57:15 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E1_C_SECONDARY_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_secondary_code_string = GALILEO_E1_C_SECONDARY_CODE ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " C " ;
2018-05-09 09:11:12 +00:00
}
else
{
d_secondary = false ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " B " ;
2018-05-09 09:11:12 +00:00
}
2019-07-10 16:02:57 +00:00
// Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD.
2018-05-09 09:11:12 +00:00
}
2020-06-19 01:22:01 +00:00
else if ( d_signal_type = = " 5X " )
2018-05-09 09:11:12 +00:00
{
2019-02-22 14:57:15 +00:00
d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ ;
2019-08-31 09:37:29 +00:00
d_code_period = GALILEO_E5A_CODE_PERIOD_S ;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_CPS ;
2018-05-09 09:11:12 +00:00
d_symbols_per_bit = 20 ;
d_correlation_length_ms = 1 ;
2019-07-10 16:02:57 +00:00
d_secondary = true ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . slope = 1.0 ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
d_trk_parameters . y_intercept = 1.0 ;
2019-07-19 09:55:53 +00:00
if ( d_extended_correlation_in_fpga = = true )
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . extend_correlation_symbols > 1 )
2019-07-19 09:55:53 +00:00
{
d_sc_demodulate_enabled = true ;
}
}
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2018-05-09 09:11:12 +00:00
{
2019-07-17 16:01:27 +00:00
// synchronize pilot secondary code
2019-02-22 14:57:15 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_Q_SECONDARY_CODE_LENGTH ) ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " Q " ;
2019-07-10 16:02:57 +00:00
// remove data secondary code
d_data_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_I_SECONDARY_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_data_secondary_code_string = GALILEO_E5A_I_SECONDARY_CODE ;
2022-02-05 14:03:25 +00:00
d_interchange_iq = true ;
2019-07-17 16:01:27 +00:00
// the pilot secondary code depends on PRN and it is initialized later
2018-05-09 09:11:12 +00:00
}
else
{
2019-07-17 16:01:27 +00:00
// synchronize and remove data secondary code
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_I_SECONDARY_CODE_LENGTH ) ;
2020-06-24 00:43:37 +00:00
d_secondary_code_string = GALILEO_E5A_I_SECONDARY_CODE ;
2020-06-19 01:22:01 +00:00
d_signal_pretty_name = d_signal_pretty_name + " I " ;
2018-05-09 09:11:12 +00:00
}
}
else
{
LOG ( WARNING ) < < " Invalid Signal argument when instantiating tracking blocks " ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Invalid Signal argument when instantiating tracking blocks \n " ;
2018-05-09 09:11:12 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
d_symbols_per_bit = 0 ;
}
}
else
{
LOG ( WARNING ) < < " Invalid System argument when instantiating tracking blocks " ;
2020-07-07 16:53:50 +00:00
std : : cerr < < " Invalid System argument when instantiating tracking blocks \n " ;
2018-05-09 09:11:12 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
d_symbols_per_bit = 0 ;
}
2018-03-02 16:40:13 +00:00
// Initialize tracking ==========================================
2020-07-10 19:13:17 +00:00
d_code_loop_filter = Tracking_loop_filter ( static_cast < float > ( d_code_period ) , d_trk_parameters . dll_bw_hz , d_trk_parameters . dll_filter_order , false ) ;
2020-06-19 01:22:01 +00:00
d_carrier_loop_filter . set_params ( d_trk_parameters . fll_bw_hz , d_trk_parameters . pll_bw_hz , d_trk_parameters . pll_filter_order ) ;
2018-03-02 16:40:13 +00:00
2019-07-17 16:01:27 +00:00
// correlator outputs (scalar)
2018-05-09 09:11:12 +00:00
if ( d_veml )
{
// Very-Early, Early, Prompt, Late, Very-Late
d_n_correlator_taps = 5 ;
}
else
{
// Early, Prompt, Late
d_n_correlator_taps = 3 ;
}
2021-10-01 10:52:29 +00:00
d_correlator_outs = volk_gnsssdr : : vector < gr_complex > ( d_n_correlator_taps ) ;
d_local_code_shift_chips = volk_gnsssdr : : vector < float > ( d_n_correlator_taps ) ;
2018-05-09 09:11:12 +00:00
// map memory pointers of correlator outputs
if ( d_veml )
{
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 ] ;
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-05-09 09:11:12 +00:00
d_local_code_shift_chips [ 2 ] = 0.0 ;
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 3 ] = d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = d_trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-05-09 09:11:12 +00:00
d_prompt_data_shift = & d_local_code_shift_chips [ 2 ] ;
}
else
2018-03-02 16:40:13 +00:00
{
2018-05-09 09:11:12 +00:00
d_Very_Early = nullptr ;
d_Early = & d_correlator_outs [ 0 ] ;
d_Prompt = & d_correlator_outs [ 1 ] ;
d_Late = & d_correlator_outs [ 2 ] ;
d_Very_Late = nullptr ;
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-05-09 09:11:12 +00:00
d_local_code_shift_chips [ 1 ] = 0.0 ;
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 2 ] = d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-05-09 09:11:12 +00:00
d_prompt_data_shift = & d_local_code_shift_chips [ 1 ] ;
2018-03-02 16:40:13 +00:00
}
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . extend_correlation_symbols > 1 )
2018-05-09 09:11:12 +00:00
{
d_enable_extended_integration = true ;
}
else
{
d_enable_extended_integration = false ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . extend_correlation_symbols = 1 ;
2018-05-09 09:11:12 +00:00
}
2018-03-02 16:40:13 +00:00
2019-02-27 16:27:31 +00:00
// --- Initializations ---
2019-04-01 10:38:45 +00:00
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
2021-10-11 13:13:12 +00:00
2018-05-09 09:11:12 +00:00
// Initial code frequency basis of NCO
d_code_freq_chips = d_code_chip_rate ;
2019-06-18 16:22:01 +00:00
d_next_integration_length_samples = d_current_integration_length_samples ;
2019-04-01 10:38:45 +00:00
2018-03-02 16:40:13 +00:00
// CN0 estimation and lock detector buffers
2021-10-01 10:52:29 +00:00
d_Prompt_buffer = volk_gnsssdr : : vector < gr_complex > ( d_trk_parameters . cn0_samples ) ;
d_Prompt_Data = volk_gnsssdr : : vector < gr_complex > ( 1 ) ;
2019-04-25 13:30:11 +00:00
d_cn0_smoother = Exponential_Smoother ( ) ;
2020-06-19 01:22:01 +00:00
d_cn0_smoother . set_alpha ( d_trk_parameters . cn0_smoother_alpha ) ;
2019-04-25 13:30:11 +00:00
if ( d_code_period > 0.0 )
{
2020-06-19 01:22:01 +00:00
d_cn0_smoother . set_samples_for_initialization ( d_trk_parameters . cn0_smoother_samples / static_cast < int > ( d_code_period * 1000.0 ) ) ;
2019-04-25 13:30:11 +00:00
}
2019-07-17 16:01:27 +00:00
d_carrier_lock_test_smoother = Exponential_Smoother ( ) ;
2020-06-19 01:22:01 +00:00
d_carrier_lock_test_smoother . set_alpha ( d_trk_parameters . carrier_lock_test_smoother_alpha ) ;
2019-07-17 16:01:27 +00:00
d_carrier_lock_test_smoother . set_min_value ( - 1.0 ) ;
d_carrier_lock_test_smoother . set_offset ( 0.0 ) ;
2020-06-19 01:22:01 +00:00
d_carrier_lock_test_smoother . set_samples_for_initialization ( d_trk_parameters . carrier_lock_test_smoother_samples ) ;
2018-03-02 16:40:13 +00:00
2018-08-02 15:32:59 +00:00
clear_tracking_vars ( ) ;
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . smoother_length > 0 )
2018-12-14 17:15:21 +00:00
{
2020-06-19 01:22:01 +00:00
d_carr_ph_history . set_capacity ( d_trk_parameters . smoother_length * 2 ) ;
d_code_ph_history . set_capacity ( d_trk_parameters . smoother_length * 2 ) ;
2018-12-14 17:15:21 +00:00
}
else
{
2019-04-01 10:38:45 +00:00
d_carr_ph_history . set_capacity ( 1 ) ;
d_code_ph_history . set_capacity ( 1 ) ;
2018-12-14 17:15:21 +00:00
}
2018-10-29 23:47:12 +00:00
2021-10-11 13:13:12 +00:00
// create multicorrelator class
int32_t * ca_codes = d_trk_parameters . ca_codes ;
int32_t * data_codes = d_trk_parameters . data_codes ;
d_multicorrelator_fpga = std : : make_shared < Fpga_Multicorrelator_8sc > ( d_n_correlator_taps , ca_codes , data_codes , d_code_length_chips , d_trk_parameters . track_pilot , d_code_samples_per_chip ) ;
d_multicorrelator_fpga - > set_output_vectors ( d_correlator_outs . data ( ) , d_Prompt_Data . data ( ) ) ;
2018-10-29 23:47:12 +00:00
if ( d_dump )
{
2020-06-19 01:22:01 +00:00
d_dump_filename = d_trk_parameters . dump_filename ;
2018-10-29 23:47:12 +00:00
std : : string dump_path ;
2018-12-14 17:15:21 +00:00
// Get path
2018-12-10 18:05:12 +00:00
if ( d_dump_filename . find_last_of ( ' / ' ) ! = std : : string : : npos )
2018-10-29 23:47:12 +00:00
{
2018-12-10 18:05:12 +00:00
std : : string dump_filename_ = d_dump_filename . substr ( d_dump_filename . find_last_of ( ' / ' ) + 1 ) ;
dump_path = d_dump_filename . substr ( 0 , d_dump_filename . find_last_of ( ' / ' ) ) ;
2023-12-21 12:57:41 +00:00
d_dump_filename = std : : move ( dump_filename_ ) ;
2018-10-29 23:47:12 +00:00
}
else
{
dump_path = std : : string ( " . " ) ;
}
if ( d_dump_filename . empty ( ) )
{
d_dump_filename = " trk_channel_ " ;
}
// remove extension if any
2018-12-10 18:05:12 +00:00
if ( d_dump_filename . substr ( 1 ) . find_last_of ( ' . ' ) ! = std : : string : : npos )
2018-10-29 23:47:12 +00:00
{
2018-12-10 18:05:12 +00:00
d_dump_filename = d_dump_filename . substr ( 0 , d_dump_filename . find_last_of ( ' . ' ) ) ;
2018-10-29 23:47:12 +00:00
}
2019-06-10 19:41:13 +00:00
d_dump_filename = dump_path + fs : : path : : preferred_separator + d_dump_filename ;
2018-10-29 23:47:12 +00:00
// create directory
if ( ! gnss_sdr_create_directory ( dump_path ) )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " GNSS-SDR cannot create dump files for the tracking block. Wrong permissions? \n " ;
2018-10-29 23:47:12 +00:00
d_dump = false ;
2018-12-14 17:15:21 +00:00
}
2018-10-29 23:47:12 +00:00
}
2018-03-02 16:40:13 +00:00
}
2019-07-19 16:23:36 +00:00
2019-03-27 15:59:04 +00:00
void dll_pll_veml_tracking_fpga : : msg_handler_telemetry_to_trk ( const pmt : : pmt_t & msg )
{
try
{
2020-07-19 07:39:32 +00:00
if ( pmt : : any_ref ( msg ) . type ( ) . hash_code ( ) = = int_type_hash_code )
2019-03-27 15:59:04 +00:00
{
2021-10-30 03:43:22 +00:00
const int tlm_event = wht : : any_cast < int > ( pmt : : any_ref ( msg ) ) ;
2019-07-19 16:23:36 +00:00
if ( tlm_event = = 1 )
2019-03-27 15:59:04 +00:00
{
2019-07-19 16:23:36 +00:00
DLOG ( INFO ) < < " Telemetry fault received in ch " < < this - > d_channel ;
gr : : thread : : scoped_lock lock ( d_setlock ) ;
2019-07-28 10:01:11 +00:00
d_carrier_lock_fail_counter = 200000 ; // force loss-of-lock condition
2019-03-27 15:59:04 +00:00
}
}
}
2021-10-30 03:43:22 +00:00
catch ( const wht : : bad_any_cast & e )
2019-03-27 15:59:04 +00:00
{
2020-08-03 06:13:21 +00:00
LOG ( WARNING ) < < " msg_handler_telemetry_to_trk Bad any_cast: " < < e . what ( ) ;
2019-03-27 15:59:04 +00:00
}
}
2018-08-12 22:54:23 +00:00
2018-05-03 08:38:21 +00:00
void dll_pll_veml_tracking_fpga : : start_tracking ( )
2018-03-02 16:40:13 +00:00
{
2019-04-04 17:55:02 +00:00
// all the calculations that do not require the data from the acquisition module are moved to the
// set_gnss_synchro command, which is received with a valid PRN before the acquisition module starts the
// acquisition process. This is done to minimize the time between the end of the acquisition process and
// the beginning of the tracking process.
2018-08-11 10:56:52 +00:00
// correct the code phase according to the delay between acq and trk
2018-03-02 16:40:13 +00:00
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 ;
2018-05-09 09:11:12 +00:00
2018-03-02 16:40:13 +00:00
d_carrier_doppler_hz = d_acq_carrier_doppler_hz ;
2020-07-05 18:20:02 +00:00
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters . fs_in ;
2019-04-02 13:53:16 +00:00
2019-04-01 10:38:45 +00:00
// filter initialization
d_carrier_loop_filter . initialize ( static_cast < float > ( d_acq_carrier_doppler_hz ) ) ; // initialize the carrier filter
2019-07-09 17:15:27 +00:00
d_corrected_doppler = false ;
2020-03-08 11:00:03 +00:00
d_acc_carrier_phase_initialized = false ;
2019-07-09 17:15:27 +00:00
2019-07-09 10:17:56 +00:00
boost : : mutex : : scoped_lock lock ( d_mutex ) ;
d_worker_is_done = true ;
2020-06-19 01:22:01 +00:00
d_m_condition . notify_one ( ) ;
2018-03-02 16:40:13 +00:00
}
2019-07-28 10:01:11 +00:00
2018-05-03 08:38:21 +00:00
dll_pll_veml_tracking_fpga : : ~ dll_pll_veml_tracking_fpga ( )
2018-03-02 16:40:13 +00:00
{
if ( d_dump_file . is_open ( ) )
{
try
2018-05-09 09:11:12 +00:00
{
2018-03-02 16:40:13 +00:00
d_dump_file . close ( ) ;
2018-05-09 09:11:12 +00:00
}
catch ( const std : : exception & ex )
{
2019-10-26 20:24:42 +00:00
LOG ( WARNING ) < < " Exception in Tracking block destructor: " < < ex . what ( ) ;
2018-05-09 09:11:12 +00:00
}
}
2018-10-29 23:47:12 +00:00
if ( d_dump_mat )
2018-05-09 09:11:12 +00:00
{
2019-02-10 20:55:51 +00:00
try
{
save_matfile ( ) ;
}
catch ( const std : : exception & ex )
{
LOG ( WARNING ) < < " Error saving the .mat file: " < < ex . what ( ) ;
}
2018-03-02 16:40:13 +00:00
}
try
2018-05-09 09:11:12 +00:00
{
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > free ( ) ;
2018-05-09 09:11:12 +00:00
}
catch ( const std : : exception & ex )
{
2019-10-26 20:24:42 +00:00
LOG ( WARNING ) < < " Exception in Tracking block destructor: " < < ex . what ( ) ;
2018-05-09 09:11:12 +00:00
}
2018-03-02 16:40:13 +00:00
}
2018-05-09 09:11:12 +00:00
bool dll_pll_veml_tracking_fpga : : acquire_secondary ( )
{
// ******* preamble correlation ********
2018-08-11 11:12:33 +00:00
int32_t corr_value = 0 ;
2018-08-11 10:56:52 +00:00
for ( uint32_t i = 0 ; i < d_secondary_code_length ; i + + )
2018-05-09 09:11:12 +00:00
{
2019-04-01 10:38:45 +00:00
if ( d_Prompt_circular_buffer [ i ] . real ( ) < 0.0 ) // symbols clipping
2018-05-09 09:11:12 +00:00
{
2020-06-24 00:43:37 +00:00
if ( d_secondary_code_string [ i ] = = ' 0 ' )
2018-05-09 09:11:12 +00:00
{
corr_value + + ;
}
else
{
corr_value - - ;
}
}
else
{
2020-06-24 00:43:37 +00:00
if ( d_secondary_code_string [ i ] = = ' 0 ' )
2018-05-09 09:11:12 +00:00
{
corr_value - - ;
}
else
{
corr_value + + ;
}
}
}
2018-08-11 11:12:33 +00:00
if ( abs ( corr_value ) = = static_cast < int32_t > ( d_secondary_code_length ) )
2018-05-09 09:11:12 +00:00
{
2021-10-05 19:38:32 +00:00
if ( corr_value < 0 )
{
d_Flag_PLL_180_deg_phase_locked = true ;
}
else
{
d_Flag_PLL_180_deg_phase_locked = false ;
}
2018-05-09 09:11:12 +00:00
return true ;
}
2019-04-01 10:38:45 +00:00
return false ;
2018-05-09 09:11:12 +00:00
}
bool dll_pll_veml_tracking_fpga : : cn0_and_tracking_lock_status ( double coh_integration_time_s )
{
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
2020-06-19 01:22:01 +00:00
if ( d_cn0_estimation_counter < d_trk_parameters . cn0_samples )
2018-05-09 09:11:12 +00:00
{
// fill buffer with prompt correlator output values
2019-08-30 15:02:17 +00:00
d_Prompt_buffer [ d_cn0_estimation_counter ] = d_P_accu ;
d_cn0_estimation_counter + + ;
2018-05-09 09:11:12 +00:00
return true ;
}
2020-06-19 01:22:01 +00:00
d_Prompt_buffer [ d_cn0_estimation_counter % d_trk_parameters . cn0_samples ] = d_P_accu ;
2019-08-30 15:02:17 +00:00
d_cn0_estimation_counter + + ;
2019-07-17 16:01:27 +00:00
// Code lock indicator
2020-07-19 07:39:32 +00:00
const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator ( d_Prompt_buffer . data ( ) , d_trk_parameters . cn0_samples , static_cast < float > ( coh_integration_time_s ) ) ;
2019-07-17 16:01:27 +00:00
d_CN0_SNV_dB_Hz = d_cn0_smoother . smooth ( d_CN0_SNV_dB_Hz_raw ) ;
// Carrier lock indicator
d_carrier_lock_test = d_carrier_lock_test_smoother . smooth ( carrier_lock_detector ( d_Prompt_buffer . data ( ) , 1 ) ) ;
// Loss of lock detection
if ( ! d_pull_in_transitory )
2018-05-09 09:11:12 +00:00
{
2019-07-17 16:01:27 +00:00
if ( d_carrier_lock_test < d_carrier_lock_threshold )
2018-05-09 09:11:12 +00:00
{
2019-07-17 16:01:27 +00:00
d_carrier_lock_fail_counter + + ;
}
else
{
if ( d_carrier_lock_fail_counter > 0 )
2019-04-01 10:38:45 +00:00
{
2019-07-17 16:01:27 +00:00
d_carrier_lock_fail_counter - - ;
2019-04-01 10:38:45 +00:00
}
2018-05-09 09:11:12 +00:00
}
2019-07-17 16:01:27 +00:00
2020-06-19 01:22:01 +00:00
if ( d_CN0_SNV_dB_Hz < d_trk_parameters . cn0_min )
2018-05-09 09:11:12 +00:00
{
2019-07-17 16:01:27 +00:00
d_code_lock_fail_counter + + ;
2018-05-09 09:11:12 +00:00
}
else
{
2019-07-17 16:01:27 +00:00
if ( d_code_lock_fail_counter > 0 )
{
d_code_lock_fail_counter - - ;
}
2018-05-09 09:11:12 +00:00
}
}
2020-06-19 01:22:01 +00:00
if ( d_carrier_lock_fail_counter > d_trk_parameters . max_carrier_lock_fail or d_code_lock_fail_counter > d_trk_parameters . max_code_lock_fail )
2019-07-17 16:01:27 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! \n " ;
2019-07-17 16:01:27 +00:00
LOG ( INFO ) < < " Loss of lock in channel " < < d_channel
< < " (carrier_lock_fail_counter: " < < d_carrier_lock_fail_counter
< < " code_lock_fail_counter : " < < d_code_lock_fail_counter < < " ) " ;
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; // 3 -> loss of lock
d_carrier_lock_fail_counter = 0 ;
d_code_lock_fail_counter = 0 ;
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > unlock_channel ( ) ;
2019-07-17 16:01:27 +00:00
return false ;
}
return true ;
2018-12-14 17:15:21 +00:00
}
// correlation requires:
// - updated remnant carrier phase in radians (rem_carr_phase_rad)
// - updated remnant code phase in samples (d_rem_code_phase_samples)
// - d_code_freq_chips
// - d_carrier_doppler_hz
2019-08-14 00:14:29 +00:00
void dll_pll_veml_tracking_fpga : : do_correlation_step ( )
2018-12-14 17:15:21 +00:00
{
2019-04-01 10:38:45 +00:00
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
2018-12-14 17:15:21 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > Carrier_wipeoff_multicorrelator_resampler (
2018-12-14 17:15:21 +00:00
d_rem_carr_phase_rad ,
2020-07-10 19:13:17 +00:00
static_cast < float > ( d_carrier_phase_step_rad ) , static_cast < float > ( d_carrier_phase_rate_step_rad ) ,
2019-02-27 16:27:31 +00:00
static_cast < float > ( d_rem_code_phase_chips ) * static_cast < float > ( d_code_samples_per_chip ) ,
static_cast < float > ( d_code_phase_step_chips ) * static_cast < float > ( d_code_samples_per_chip ) ,
static_cast < float > ( d_code_phase_rate_step_chips ) * static_cast < float > ( d_code_samples_per_chip ) ,
2019-07-19 09:55:53 +00:00
d_current_integration_length_samples ) ;
2018-05-09 09:11:12 +00:00
}
void dll_pll_veml_tracking_fpga : : run_dll_pll ( )
2018-03-02 16:40:13 +00:00
{
2018-05-09 09:11:12 +00:00
// ################## PLL ##########################################################
// PLL discriminator
if ( d_cloop )
{
// Costas loop discriminator, insensitive to 180 deg phase transitions
2020-07-05 18:20:02 +00:00
d_carr_phase_error_hz = pll_cloop_two_quadrant_atan ( d_P_accu ) / TWO_PI ;
2018-05-09 09:11:12 +00:00
}
else
{
// Secondary code acquired. No symbols transition should be present in the signal
2020-07-05 18:20:02 +00:00
d_carr_phase_error_hz = pll_four_quadrant_atan ( d_P_accu ) / TWO_PI ;
2018-05-09 09:11:12 +00:00
}
2018-03-02 16:40:13 +00:00
2020-06-19 01:22:01 +00:00
if ( ( d_pull_in_transitory = = true and d_trk_parameters . enable_fll_pull_in = = true ) or d_trk_parameters . enable_fll_steady_state )
2019-04-01 10:38:45 +00:00
{
// FLL discriminator
2020-07-05 18:20:02 +00:00
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_carr_freq_error_hz = fll_diff_atan ( d_P_accu_old , d_P_accu , 0 , d_current_correlation_time_s ) / TWO_PI ;
2019-07-17 16:01:27 +00:00
2019-04-01 10:38:45 +00:00
d_P_accu_old = d_P_accu ;
2020-07-07 16:53:50 +00:00
// std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << '\n';
2019-04-01 10:38:45 +00:00
// Carrier discriminator filter
2020-06-19 01:22:01 +00:00
if ( ( d_pull_in_transitory = = true and d_trk_parameters . enable_fll_pull_in = = true ) )
2019-04-01 10:38:45 +00:00
{
2019-07-17 16:01:27 +00:00
// pure FLL, disable PLL
2020-07-10 19:13:17 +00:00
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( static_cast < float > ( d_carr_freq_error_hz ) , 0 , static_cast < float > ( d_current_correlation_time_s ) ) ;
2019-04-01 10:38:45 +00:00
}
else
{
2019-07-17 16:01:27 +00:00
// FLL-aided PLL
2020-07-10 19:13:17 +00:00
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( static_cast < float > ( d_carr_freq_error_hz ) , static_cast < float > ( d_carr_phase_error_hz ) , static_cast < float > ( d_current_correlation_time_s ) ) ;
2019-04-01 10:38:45 +00:00
}
}
else
{
// Carrier discriminator filter
2020-07-10 19:13:17 +00:00
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( 0 , static_cast < float > ( d_carr_phase_error_hz ) , static_cast < float > ( d_current_correlation_time_s ) ) ;
2019-04-01 10:38:45 +00:00
}
2018-05-09 09:11:12 +00:00
// New carrier Doppler frequency estimation
2019-04-01 10:38:45 +00:00
d_carrier_doppler_hz = d_carr_error_filt_hz ;
2018-12-14 17:15:21 +00:00
2020-07-07 16:53:50 +00:00
// std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n';
// std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << '\n';
2019-07-17 16:01:27 +00:00
2018-05-09 09:11:12 +00:00
// ################## DLL ##########################################################
// DLL discriminator
if ( d_veml )
{
d_code_error_chips = dll_nc_vemlp_normalized ( d_VE_accu , d_E_accu , d_L_accu , d_VL_accu ) ; // [chips/Ti]
}
else
{
2020-06-19 01:22:01 +00:00
d_code_error_chips = dll_nc_e_minus_l_normalized ( d_E_accu , d_L_accu , d_trk_parameters . spc , d_trk_parameters . slope , d_trk_parameters . y_intercept ) ; // [chips/Ti]
2018-05-09 09:11:12 +00:00
}
// Code discriminator filter
2020-07-10 19:13:17 +00:00
d_code_error_filt_chips = d_code_loop_filter . apply ( static_cast < float > ( d_code_error_chips ) ) ; // [chips/second]
2018-12-14 17:15:21 +00:00
// New code Doppler frequency estimation
2020-02-24 19:18:28 +00:00
d_code_freq_chips = d_code_chip_rate - d_code_error_filt_chips ;
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . carrier_aiding )
2020-02-24 19:18:28 +00:00
{
d_code_freq_chips + = d_carrier_doppler_hz * d_code_chip_rate / d_signal_carrier_freq ;
}
2019-07-09 17:15:27 +00:00
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . enable_doppler_correction = = true )
2019-07-09 17:15:27 +00:00
{
if ( d_pull_in_transitory = = false and d_corrected_doppler = = false )
{
d_dll_filt_history . push_back ( static_cast < float > ( d_code_error_filt_chips ) ) ;
if ( d_dll_filt_history . full ( ) )
{
2020-07-19 07:39:32 +00:00
const float avg_code_error_chips_s = static_cast < float > ( std : : accumulate ( d_dll_filt_history . begin ( ) , d_dll_filt_history . end ( ) , 0.0 ) ) / static_cast < float > ( d_dll_filt_history . capacity ( ) ) ;
2021-10-11 13:13:12 +00:00
if ( std : : fabs ( avg_code_error_chips_s ) > 1.0 )
2019-07-09 17:15:27 +00:00
{
2020-07-19 07:39:32 +00:00
const float carrier_doppler_error_hz = static_cast < float > ( d_signal_carrier_freq ) * avg_code_error_chips_s / static_cast < float > ( d_code_chip_rate ) ;
2020-06-19 01:22:01 +00:00
LOG ( INFO ) < < " Detected and corrected carrier doppler error: " < < carrier_doppler_error_hz < < " [Hz] on sat " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) ;
2020-07-10 19:13:17 +00:00
d_carrier_loop_filter . initialize ( static_cast < float > ( d_carrier_doppler_hz ) - carrier_doppler_error_hz ) ;
2019-07-09 17:15:27 +00:00
d_corrected_doppler = true ;
}
d_dll_filt_history . clear ( ) ;
}
}
}
2018-05-09 09:11:12 +00:00
}
2018-03-02 16:40:13 +00:00
2020-03-08 11:00:03 +00:00
void dll_pll_veml_tracking_fpga : : check_carrier_phase_coherent_initialization ( )
{
if ( d_acc_carrier_phase_initialized = = false )
{
d_acc_carrier_phase_rad = - d_rem_carr_phase_rad ;
d_acc_carrier_phase_initialized = true ;
}
}
2018-05-09 09:11:12 +00:00
void dll_pll_veml_tracking_fpga : : clear_tracking_vars ( )
{
2019-10-26 20:24:42 +00:00
std : : fill_n ( d_correlator_outs . begin ( ) , d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-04-01 10:38:45 +00:00
{
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
2019-07-10 16:02:57 +00:00
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
2019-04-01 10:38:45 +00:00
}
d_P_accu_old = gr_complex ( 0.0 , 0.0 ) ;
d_carr_phase_error_hz = 0.0 ;
d_carr_freq_error_hz = 0.0 ;
2018-05-09 09:11:12 +00:00
d_carr_error_filt_hz = 0.0 ;
d_code_error_chips = 0.0 ;
d_code_error_filt_chips = 0.0 ;
d_current_symbol = 0 ;
2019-07-10 16:02:57 +00:00
d_current_data_symbol = 0 ;
2019-04-01 10:38:45 +00:00
d_Prompt_circular_buffer . clear ( ) ;
2018-12-14 17:15:21 +00:00
d_carrier_phase_rate_step_rad = 0.0 ;
d_code_phase_rate_step_chips = 0.0 ;
d_carr_ph_history . clear ( ) ;
d_code_ph_history . clear ( ) ;
2018-05-09 09:11:12 +00:00
}
void dll_pll_veml_tracking_fpga : : update_tracking_vars ( )
{
2020-06-19 01:22:01 +00:00
d_T_chip_seconds = 1.0 / d_code_freq_chips ;
d_T_prn_seconds = d_T_chip_seconds * static_cast < double > ( d_code_length_chips ) ;
2019-06-18 16:22:01 +00:00
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
2020-06-19 01:22:01 +00:00
d_T_prn_samples = d_T_prn_seconds * d_trk_parameters . fs_in ;
d_K_blk_samples = d_T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples ; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function
2020-07-19 07:39:32 +00:00
const auto actual_blk_length = static_cast < int32_t > ( std : : floor ( d_K_blk_samples ) ) ;
2019-07-22 09:47:33 +00:00
d_next_integration_length_samples = actual_blk_length ;
2019-07-28 10:01:11 +00:00
// ################## PLL COMMANDS #################################################
2019-06-18 16:22:01 +00:00
// carrier phase step (NCO phase increment per sample) [rads/sample]
2020-07-05 18:20:02 +00:00
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters . fs_in ;
2019-06-18 16:22:01 +00:00
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . high_dyn )
2019-06-18 16:22:01 +00:00
{
d_carr_ph_history . push_back ( std : : pair < double , double > ( d_carrier_phase_step_rad , static_cast < double > ( d_current_integration_length_samples ) ) ) ;
if ( d_carr_ph_history . full ( ) )
{
double tmp_cp1 = 0.0 ;
double tmp_cp2 = 0.0 ;
double tmp_samples = 0.0 ;
2020-06-19 01:22:01 +00:00
for ( unsigned int k = 0 ; k < d_trk_parameters . smoother_length ; k + + )
2019-06-18 16:22:01 +00:00
{
tmp_cp1 + = d_carr_ph_history [ k ] . first ;
2020-06-19 01:22:01 +00:00
tmp_cp2 + = d_carr_ph_history [ d_trk_parameters . smoother_length * 2 - k - 1 ] . first ;
tmp_samples + = d_carr_ph_history [ d_trk_parameters . smoother_length * 2 - k - 1 ] . second ;
2019-06-18 16:22:01 +00:00
}
2020-06-19 01:22:01 +00:00
tmp_cp1 / = static_cast < double > ( d_trk_parameters . smoother_length ) ;
tmp_cp2 / = static_cast < double > ( d_trk_parameters . smoother_length ) ;
2019-06-18 16:22:01 +00:00
d_carrier_phase_rate_step_rad = ( tmp_cp2 - tmp_cp1 ) / tmp_samples ;
}
}
2020-07-07 16:53:50 +00:00
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI << '\n';
2019-07-17 16:01:27 +00:00
// remnant carrier phase to prevent overflow in the code NCO
2019-06-18 16:22:01 +00:00
d_rem_carr_phase_rad + = static_cast < float > ( d_carrier_phase_step_rad * static_cast < double > ( d_current_integration_length_samples ) + 0.5 * d_carrier_phase_rate_step_rad * static_cast < double > ( d_current_integration_length_samples ) * static_cast < double > ( d_current_integration_length_samples ) ) ;
2020-07-05 18:20:02 +00:00
d_rem_carr_phase_rad = fmod ( d_rem_carr_phase_rad , TWO_PI ) ;
2019-06-18 16:22:01 +00:00
2019-07-17 16:01:27 +00:00
// carrier phase accumulator
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
2020-07-07 16:53:50 +00:00
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n';
2019-06-18 16:22:01 +00:00
d_acc_carrier_phase_rad - = ( d_carrier_phase_step_rad * static_cast < double > ( d_current_integration_length_samples ) + 0.5 * d_carrier_phase_rate_step_rad * static_cast < double > ( d_current_integration_length_samples ) * static_cast < double > ( d_current_integration_length_samples ) ) ;
2019-07-28 10:01:11 +00:00
// ################## DLL COMMANDS #################################################
2019-06-18 16:22:01 +00:00
// code phase step (Code resampler phase increment per sample) [chips/sample]
2020-06-19 01:22:01 +00:00
d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters . fs_in ;
if ( d_trk_parameters . high_dyn )
2019-06-18 16:22:01 +00:00
{
d_code_ph_history . push_back ( std : : pair < double , double > ( d_code_phase_step_chips , static_cast < double > ( d_current_integration_length_samples ) ) ) ;
if ( d_code_ph_history . full ( ) )
{
double tmp_cp1 = 0.0 ;
double tmp_cp2 = 0.0 ;
double tmp_samples = 0.0 ;
2020-06-19 01:22:01 +00:00
for ( unsigned int k = 0 ; k < d_trk_parameters . smoother_length ; k + + )
2019-06-18 16:22:01 +00:00
{
tmp_cp1 + = d_code_ph_history [ k ] . first ;
2020-06-19 01:22:01 +00:00
tmp_cp2 + = d_code_ph_history [ d_trk_parameters . smoother_length * 2 - k - 1 ] . first ;
tmp_samples + = d_code_ph_history [ d_trk_parameters . smoother_length * 2 - k - 1 ] . second ;
2019-06-18 16:22:01 +00:00
}
2020-06-19 01:22:01 +00:00
tmp_cp1 / = static_cast < double > ( d_trk_parameters . smoother_length ) ;
tmp_cp2 / = static_cast < double > ( d_trk_parameters . smoother_length ) ;
2019-06-18 16:22:01 +00:00
d_code_phase_rate_step_chips = ( tmp_cp2 - tmp_cp1 ) / tmp_samples ;
}
}
// remnant code phase [chips]
d_rem_code_phase_samples_prev = d_rem_code_phase_samples ;
2020-06-19 01:22:01 +00:00
d_rem_code_phase_samples = d_K_blk_samples - static_cast < double > ( d_current_integration_length_samples ) ; // rounding error < 1 sample
d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / d_trk_parameters . fs_in ;
2019-06-18 16:22:01 +00:00
}
2018-05-09 09:11:12 +00:00
void dll_pll_veml_tracking_fpga : : save_correlation_results ( )
{
2019-07-19 09:55:53 +00:00
if ( d_secondary & & ( ! d_current_extended_correlation_in_fpga ) ) // the FPGA removes the secondary code
2018-03-02 16:40:13 +00:00
{
2020-06-24 00:43:37 +00:00
if ( d_secondary_code_string [ d_current_symbol ] = = ' 0 ' )
2018-03-02 16:40:13 +00:00
{
2018-05-09 09:11:12 +00:00
if ( d_veml )
{
d_VE_accu + = * d_Very_Early ;
d_VL_accu + = * d_Very_Late ;
}
d_E_accu + = * d_Early ;
d_P_accu + = * d_Prompt ;
d_L_accu + = * d_Late ;
2018-03-02 16:40:13 +00:00
}
else
{
2018-05-09 09:11:12 +00:00
if ( d_veml )
{
d_VE_accu - = * d_Very_Early ;
d_VL_accu - = * d_Very_Late ;
}
d_E_accu - = * d_Early ;
d_P_accu - = * d_Prompt ;
d_L_accu - = * d_Late ;
}
d_current_symbol + + ;
// secondary code roll-up
d_current_symbol % = d_secondary_code_length ;
}
else
{
if ( d_veml )
{
d_VE_accu + = * d_Very_Early ;
d_VL_accu + = * d_Very_Late ;
}
d_E_accu + = * d_Early ;
d_P_accu + = * d_Prompt ;
d_L_accu + = * d_Late ;
}
2019-07-10 16:02:57 +00:00
// data secondary code roll-up
if ( d_symbols_per_bit > 1 )
{
if ( d_data_secondary_code_length > 0 )
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-07-10 16:02:57 +00:00
{
2019-07-19 09:55:53 +00:00
if ( ! d_current_extended_correlation_in_fpga ) // the FPGA removes the secondary code
{
2020-06-24 00:43:37 +00:00
if ( d_data_secondary_code_string [ d_current_data_symbol ] = = ' 0 ' )
2019-07-19 09:55:53 +00:00
{
2019-10-26 20:24:42 +00:00
d_P_data_accu + = d_Prompt_Data [ 0 ] ;
2019-07-19 09:55:53 +00:00
}
else
{
2019-10-26 20:24:42 +00:00
d_P_data_accu - = d_Prompt_Data [ 0 ] ;
2019-07-19 09:55:53 +00:00
}
}
else
{
2019-10-26 20:24:42 +00:00
d_P_data_accu + = d_Prompt_Data [ 0 ] ;
2019-07-19 09:55:53 +00:00
}
2019-07-10 16:02:57 +00:00
}
else
{
2019-07-19 09:55:53 +00:00
if ( ! d_current_extended_correlation_in_fpga )
{
2020-06-24 00:43:37 +00:00
if ( d_data_secondary_code_string [ d_current_data_symbol ] = = ' 0 ' )
2019-07-19 09:55:53 +00:00
{
d_P_data_accu + = * d_Prompt ;
}
else
{
d_P_data_accu - = * d_Prompt ;
}
}
else
{
d_P_data_accu + = * d_Prompt ;
}
2019-07-10 16:02:57 +00:00
}
2019-07-17 16:01:27 +00:00
d_current_data_symbol + = d_current_fpga_integration_period ;
2019-07-10 16:02:57 +00:00
d_current_data_symbol % = d_data_secondary_code_length ;
}
else
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-07-10 16:02:57 +00:00
{
2019-10-26 20:24:42 +00:00
d_P_data_accu + = d_Prompt_Data [ 0 ] ;
2019-07-10 16:02:57 +00:00
}
else
{
d_P_data_accu + = * d_Prompt ;
2020-07-07 16:53:50 +00:00
// std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << '\n';
2019-07-10 16:02:57 +00:00
}
2019-07-17 16:01:27 +00:00
d_current_data_symbol + = d_current_fpga_integration_period ;
2019-07-10 16:02:57 +00:00
d_current_data_symbol % = d_symbols_per_bit ;
}
}
else
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-07-10 16:02:57 +00:00
{
2019-10-26 20:24:42 +00:00
d_P_data_accu = d_Prompt_Data [ 0 ] ;
2019-07-10 16:02:57 +00:00
}
else
{
d_P_data_accu = * d_Prompt ;
}
}
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-04-01 10:38:45 +00:00
{
2019-07-17 16:01:27 +00:00
// If tracking pilot, disable Costas loop
2019-04-01 10:38:45 +00:00
d_cloop = false ;
}
2018-05-09 09:11:12 +00:00
else
2019-04-01 10:38:45 +00:00
{
d_cloop = true ;
}
2018-05-09 09:11:12 +00:00
}
2019-07-09 18:11:21 +00:00
void dll_pll_veml_tracking_fpga : : log_data ( )
2018-05-09 09:11:12 +00:00
{
2018-10-29 23:47:12 +00:00
if ( d_dump )
2018-05-09 09:11:12 +00:00
{
// Dump results to file
float prompt_I ;
float prompt_Q ;
2019-08-12 22:19:31 +00:00
float tmp_VE ;
float tmp_E ;
float tmp_P ;
float tmp_L ;
float tmp_VL ;
2018-05-09 09:11:12 +00:00
float tmp_float ;
double tmp_double ;
2018-08-12 22:54:23 +00:00
uint64_t tmp_long_int ;
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2018-05-09 09:11:12 +00:00
{
2019-10-26 20:24:42 +00:00
prompt_I = d_Prompt_Data . data ( ) - > real ( ) ;
prompt_Q = d_Prompt_Data . data ( ) - > imag ( ) ;
2018-03-02 16:40:13 +00:00
}
else
{
2019-07-17 16:01:27 +00:00
prompt_I = d_Prompt - > real ( ) ;
prompt_Q = d_Prompt - > imag ( ) ;
2018-05-09 09:11:12 +00:00
}
if ( d_veml )
{
tmp_VE = std : : abs < float > ( d_VE_accu ) ;
tmp_VL = std : : abs < float > ( d_VL_accu ) ;
}
else
{
tmp_VE = 0.0 ;
tmp_VL = 0.0 ;
}
tmp_E = std : : abs < float > ( d_E_accu ) ;
tmp_P = std : : abs < float > ( d_P_accu ) ;
tmp_L = std : : abs < float > ( d_L_accu ) ;
2018-03-02 16:40:13 +00:00
2018-05-09 09:11:12 +00:00
try
{
// Dump correlators output
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VE ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_E ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_P ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_L ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VL ) , sizeof ( float ) ) ;
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_I ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_Q ) , sizeof ( float ) ) ;
// PRN start sample stamp
2019-06-18 16:22:01 +00:00
tmp_long_int = d_sample_counter_next ;
2018-08-11 10:56:52 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_long_int ) , sizeof ( uint64_t ) ) ;
2018-05-09 09:11:12 +00:00
// accumulated carrier phase
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_acc_carrier_phase_rad ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// carrier and code frequency
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_doppler_hz ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-12-14 17:15:21 +00:00
// carrier phase rate [Hz/s]
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_phase_rate_step_rad * d_trk_parameters . fs_in * d_trk_parameters . fs_in / TWO_PI ) ;
2018-12-14 17:15:21 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_code_freq_chips ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-12-14 17:15:21 +00:00
// code phase rate [chips/s^2]
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_code_phase_rate_step_chips * d_trk_parameters . fs_in * d_trk_parameters . fs_in ) ;
2018-12-14 17:15:21 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-05-09 09:11:12 +00:00
// PLL commands
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carr_phase_error_hz ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carr_error_filt_hz ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// DLL commands
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_code_error_chips ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_code_error_filt_chips ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// CN0 and carrier lock test
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_CN0_SNV_dB_Hz ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_lock_test ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// AUX vars (for debug purposes)
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_rem_code_phase_samples ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2019-07-17 16:01:27 +00:00
tmp_double = static_cast < double > ( d_sample_counter_next ) ;
2018-05-09 09:11:12 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
// PRN
2018-08-11 10:56:52 +00:00
uint32_t prn_ = d_acquisition_gnss_synchro - > PRN ;
d_dump_file . write ( reinterpret_cast < char * > ( & prn_ ) , sizeof ( uint32_t ) ) ;
2018-05-09 09:11:12 +00:00
}
2022-12-11 11:48:00 +00:00
catch ( const std : : ofstream : : failure & e )
2018-05-09 09:11:12 +00:00
{
LOG ( WARNING ) < < " Exception writing trk dump file " < < e . what ( ) ;
}
2018-03-02 16:40:13 +00:00
}
2018-05-09 09:11:12 +00:00
}
2020-07-01 17:52:52 +00:00
int32_t dll_pll_veml_tracking_fpga : : save_matfile ( ) const
2018-05-09 09:11:12 +00:00
{
// READ DUMP FILE
std : : ifstream : : pos_type size ;
2020-07-19 07:39:32 +00:00
const int32_t number_of_double_vars = 1 ;
const int32_t number_of_float_vars = 19 ;
const int32_t epoch_size_bytes = sizeof ( uint64_t ) + sizeof ( double ) * number_of_double_vars +
sizeof ( float ) * number_of_float_vars + sizeof ( uint32_t ) ;
2018-05-09 09:11:12 +00:00
std : : ifstream dump_file ;
2018-10-29 23:47:12 +00:00
std : : string dump_filename_ = d_dump_filename ;
// add channel number to the filename
2018-11-04 21:50:46 +00:00
dump_filename_ . append ( std : : to_string ( d_channel ) ) ;
2018-10-29 23:47:12 +00:00
// add extension
dump_filename_ . append ( " .dat " ) ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Generating .mat file for " < < dump_filename_ < < ' \n ' ;
2018-05-09 09:11:12 +00:00
dump_file . exceptions ( std : : ifstream : : failbit | std : : ifstream : : badbit ) ;
try
{
2018-10-29 23:47:12 +00:00
dump_file . open ( dump_filename_ . c_str ( ) , std : : ios : : binary | std : : ios : : ate ) ;
2018-05-09 09:11:12 +00:00
}
catch ( const std : : ifstream : : failure & e )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " Problem opening dump file: " < < e . what ( ) < < ' \n ' ;
2018-05-09 09:11:12 +00:00
return 1 ;
}
// count number of epochs and rewind
2018-08-10 18:34:03 +00:00
int64_t num_epoch = 0 ;
2018-05-09 09:11:12 +00:00
if ( dump_file . is_open ( ) )
2018-03-02 16:40:13 +00:00
{
2018-05-09 09:11:12 +00:00
size = dump_file . tellg ( ) ;
2018-08-10 18:34:03 +00:00
num_epoch = static_cast < int64_t > ( size ) / static_cast < int64_t > ( epoch_size_bytes ) ;
2018-05-09 09:11:12 +00:00
dump_file . seekg ( 0 , std : : ios : : beg ) ;
2018-03-02 16:40:13 +00:00
}
else
{
2018-05-09 09:11:12 +00:00
return 1 ;
2018-03-02 16:40:13 +00:00
}
2019-06-30 12:32:40 +00:00
auto abs_VE = std : : vector < float > ( num_epoch ) ;
auto abs_E = std : : vector < float > ( num_epoch ) ;
auto abs_P = std : : vector < float > ( num_epoch ) ;
auto abs_L = std : : vector < float > ( num_epoch ) ;
auto abs_VL = std : : vector < float > ( num_epoch ) ;
auto Prompt_I = std : : vector < float > ( num_epoch ) ;
auto Prompt_Q = std : : vector < float > ( num_epoch ) ;
auto PRN_start_sample_count = std : : vector < uint64_t > ( num_epoch ) ;
auto acc_carrier_phase_rad = std : : vector < float > ( num_epoch ) ;
auto carrier_doppler_hz = std : : vector < float > ( num_epoch ) ;
auto carrier_doppler_rate_hz = std : : vector < float > ( num_epoch ) ;
auto code_freq_chips = std : : vector < float > ( num_epoch ) ;
auto code_freq_rate_chips = std : : vector < float > ( num_epoch ) ;
auto carr_error_hz = std : : vector < float > ( num_epoch ) ;
auto carr_error_filt_hz = std : : vector < float > ( num_epoch ) ;
auto code_error_chips = std : : vector < float > ( num_epoch ) ;
auto code_error_filt_chips = std : : vector < float > ( num_epoch ) ;
auto CN0_SNV_dB_Hz = std : : vector < float > ( num_epoch ) ;
auto carrier_lock_test = std : : vector < float > ( num_epoch ) ;
auto aux1 = std : : vector < float > ( num_epoch ) ;
auto aux2 = std : : vector < double > ( num_epoch ) ;
auto PRN = std : : vector < uint32_t > ( num_epoch ) ;
2018-05-09 09:11:12 +00:00
try
{
if ( dump_file . is_open ( ) )
{
2018-08-10 18:34:03 +00:00
for ( int64_t i = 0 ; i < num_epoch ; i + + )
2018-05-09 09:11:12 +00:00
{
dump_file . read ( reinterpret_cast < char * > ( & abs_VE [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & abs_E [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & abs_P [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & abs_L [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & abs_VL [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & Prompt_I [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & Prompt_Q [ i ] ) , sizeof ( float ) ) ;
2018-08-10 18:34:03 +00:00
dump_file . read ( reinterpret_cast < char * > ( & PRN_start_sample_count [ i ] ) , sizeof ( uint64_t ) ) ;
2018-05-09 09:11:12 +00:00
dump_file . read ( reinterpret_cast < char * > ( & acc_carrier_phase_rad [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & carrier_doppler_hz [ i ] ) , sizeof ( float ) ) ;
2018-12-14 17:15:21 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carrier_doppler_rate_hz [ i ] ) , sizeof ( float ) ) ;
2018-05-09 09:11:12 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_freq_chips [ i ] ) , sizeof ( float ) ) ;
2018-12-14 17:15:21 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_freq_rate_chips [ i ] ) , sizeof ( float ) ) ;
2018-05-09 09:11:12 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carr_error_hz [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & carr_error_filt_hz [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & code_error_chips [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & code_error_filt_chips [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & CN0_SNV_dB_Hz [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & carrier_lock_test [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & aux1 [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & aux2 [ i ] ) , sizeof ( double ) ) ;
2018-08-11 10:56:52 +00:00
dump_file . read ( reinterpret_cast < char * > ( & PRN [ i ] ) , sizeof ( uint32_t ) ) ;
2018-05-09 09:11:12 +00:00
}
}
dump_file . close ( ) ;
}
catch ( const std : : ifstream : : failure & e )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " Problem reading dump file: " < < e . what ( ) < < ' \n ' ;
2018-05-09 09:11:12 +00:00
return 1 ;
}
2018-03-02 16:40:13 +00:00
2018-05-09 09:11:12 +00:00
// WRITE MAT FILE
mat_t * matfp ;
matvar_t * matvar ;
2023-11-29 22:45:38 +00:00
std : : string filename = std : : move ( dump_filename_ ) ;
2018-05-09 09:11:12 +00:00
filename . erase ( filename . length ( ) - 4 , 4 ) ;
filename . append ( " .mat " ) ;
2018-12-10 18:05:12 +00:00
matfp = Mat_CreateVer ( filename . c_str ( ) , nullptr , MAT_FT_MAT73 ) ;
if ( reinterpret_cast < int64_t * > ( matfp ) ! = nullptr )
2018-05-09 09:11:12 +00:00
{
2019-07-14 12:09:12 +00:00
std : : array < size_t , 2 > dims { 1 , static_cast < size_t > ( num_epoch ) } ;
matvar = Mat_VarCreate ( " abs_VE " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_VE . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " abs_E " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_E . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " abs_P " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_P . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " abs_L " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_L . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " abs_VL " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_VL . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " Prompt_I " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , Prompt_I . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " Prompt_Q " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , Prompt_Q . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " PRN_start_sample_count " , MAT_C_UINT64 , MAT_T_UINT64 , 2 , dims . data ( ) , PRN_start_sample_count . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " acc_carrier_phase_rad " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , acc_carrier_phase_rad . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " carrier_doppler_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_doppler_hz . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " carrier_doppler_rate_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_doppler_rate_hz . data ( ) , 0 ) ;
2018-12-14 17:15:21 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " code_freq_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_freq_chips . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " code_freq_rate_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_freq_rate_chips . data ( ) , 0 ) ;
2018-12-14 17:15:21 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " carr_error_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carr_error_hz . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " carr_error_filt_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carr_error_filt_hz . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " code_error_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_error_chips . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " code_error_filt_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_error_filt_chips . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " CN0_SNV_dB_Hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , CN0_SNV_dB_Hz . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " carrier_lock_test " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_lock_test . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " aux1 " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , aux1 . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " aux2 " , MAT_C_DOUBLE , MAT_T_DOUBLE , 2 , dims . data ( ) , aux2 . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " PRN " , MAT_C_UINT32 , MAT_T_UINT32 , 2 , dims . data ( ) , PRN . data ( ) , 0 ) ;
2018-05-09 09:11:12 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
}
Mat_Close ( matfp ) ;
return 0 ;
}
2018-03-02 16:40:13 +00:00
2018-08-12 22:54:23 +00:00
2021-01-25 23:14:50 +00:00
void dll_pll_veml_tracking_fpga : : set_channel ( uint32_t channel , const std : : string & device_io_name )
2018-03-02 16:40:13 +00:00
{
2019-07-24 09:26:16 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2018-03-02 16:40:13 +00:00
d_channel = channel ;
2020-11-27 09:51:06 +00:00
d_multicorrelator_fpga - > open_channel ( device_io_name , channel ) ;
2018-03-02 16:40:13 +00:00
LOG ( INFO ) < < " Tracking Channel set to " < < d_channel ;
// ############# ENABLE DATA FILE LOG #################
2018-10-29 23:47:12 +00:00
if ( d_dump )
2018-03-02 16:40:13 +00:00
{
2018-10-29 23:47:12 +00:00
std : : string dump_filename_ = d_dump_filename ;
// add channel number to the filename
2018-11-04 21:50:46 +00:00
dump_filename_ . append ( std : : to_string ( d_channel ) ) ;
2018-10-29 23:47:12 +00:00
// add extension
dump_filename_ . append ( " .dat " ) ;
2018-05-09 09:11:12 +00:00
if ( ! d_dump_file . is_open ( ) )
2018-03-02 16:40:13 +00:00
{
try
2018-05-09 09:11:12 +00:00
{
2022-12-11 11:48:00 +00:00
d_dump_file . exceptions ( std : : ofstream : : failbit | std : : ofstream : : badbit ) ;
2018-10-29 23:47:12 +00:00
d_dump_file . open ( dump_filename_ . c_str ( ) , std : : ios : : out | std : : ios : : binary ) ;
LOG ( INFO ) < < " Tracking dump enabled on channel " < < d_channel < < " Log file: " < < dump_filename_ . c_str ( ) ;
2018-05-09 09:11:12 +00:00
}
2022-12-11 11:48:00 +00:00
catch ( const std : : ofstream : : failure & e )
2018-05-09 09:11:12 +00:00
{
2018-03-02 16:40:13 +00:00
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening trk dump file " < < e . what ( ) ;
2018-05-09 09:11:12 +00:00
}
2018-03-02 16:40:13 +00:00
}
}
2019-07-19 09:55:53 +00:00
if ( d_enable_extended_integration = = true )
{
if ( d_extended_correlation_in_fpga = = true )
{
// Now we can write the secondary codes that do not depend on the PRN number
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . system = = ' G ' )
2019-07-19 09:55:53 +00:00
{
2020-06-19 01:22:01 +00:00
if ( d_signal_type = = " L5 " )
2019-07-19 09:55:53 +00:00
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-07-19 09:55:53 +00:00
{
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > set_secondary_code_lengths ( d_secondary_code_length , d_data_secondary_code_length ) ;
2020-06-24 00:43:37 +00:00
d_multicorrelator_fpga - > initialize_secondary_code ( 0 , & d_secondary_code_string ) ;
d_multicorrelator_fpga - > initialize_secondary_code ( 1 , & d_data_secondary_code_string ) ;
2019-07-19 09:55:53 +00:00
}
else
{
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > set_secondary_code_lengths ( d_secondary_code_length , 0 ) ;
2020-06-24 00:43:37 +00:00
d_multicorrelator_fpga - > initialize_secondary_code ( 0 , & d_secondary_code_string ) ;
2019-07-19 09:55:53 +00:00
}
}
}
2020-06-19 01:22:01 +00:00
else if ( d_trk_parameters . system = = ' E ' )
2019-07-19 09:55:53 +00:00
{
2020-06-19 01:22:01 +00:00
if ( d_signal_type = = " 5X " )
2019-07-19 09:55:53 +00:00
{
// coherent integration in the FPGA is only enabled when tracking the pilot.
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-07-19 09:55:53 +00:00
{
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > set_secondary_code_lengths ( d_secondary_code_length , d_data_secondary_code_length ) ;
2020-06-24 00:43:37 +00:00
d_multicorrelator_fpga - > initialize_secondary_code ( 1 , & d_data_secondary_code_string ) ;
2019-07-19 09:55:53 +00:00
}
}
}
}
}
2018-03-02 16:40:13 +00:00
}
2018-08-12 22:54:23 +00:00
2018-05-09 11:09:26 +00:00
void dll_pll_veml_tracking_fpga : : set_gnss_synchro ( Gnss_Synchro * p_gnss_synchro )
2018-03-02 16:40:13 +00:00
{
d_acquisition_gnss_synchro = p_gnss_synchro ;
2019-03-22 18:03:46 +00:00
if ( p_gnss_synchro - > PRN > 0 )
{
2023-12-21 12:57:41 +00:00
gr : : thread : : scoped_lock lock ( d_setlock ) ;
2019-07-19 09:55:53 +00:00
// A set_gnss_synchro command with a valid PRN is received when the system is going to run acquisition with that PRN.
2019-06-20 12:28:47 +00:00
// We can use this command to pre-initialize tracking parameters and variables before the actual acquisition process takes place.
2019-07-19 09:55:53 +00:00
// In this way we minimize the latency between acquisition and tracking once the acquisition has been made.
2019-04-01 10:38:45 +00:00
d_sample_counter = 0 ;
d_sample_counter_next = 0 ;
2019-04-02 13:53:16 +00:00
d_carrier_phase_rate_step_rad = 0.0 ;
2019-03-22 18:03:46 +00:00
d_code_ph_history . clear ( ) ;
2019-04-01 10:38:45 +00:00
d_carr_ph_history . clear ( ) ;
2019-03-22 18:03:46 +00:00
2020-07-10 19:13:17 +00:00
if ( ( d_systemName = = " GPS " and d_signal_type = = " L5 " ) | | ( d_systemName = = " Galileo " and d_signal_type = = " 1B " ) )
2019-03-22 18:03:46 +00:00
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-03-22 18:03:46 +00:00
{
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
}
}
2020-06-19 01:22:01 +00:00
else if ( d_systemName = = " Galileo " and d_signal_type = = " 5X " )
2019-03-22 18:03:46 +00:00
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . track_pilot )
2019-03-22 18:03:46 +00:00
{
2020-06-24 00:43:37 +00:00
d_secondary_code_string = GALILEO_E5A_Q_SECONDARY_CODE [ d_acquisition_gnss_synchro - > PRN - 1 ] ;
2019-06-18 16:22:01 +00:00
2019-03-22 18:03:46 +00:00
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
2019-06-18 16:22:01 +00:00
2019-07-19 09:55:53 +00:00
if ( d_enable_extended_integration = = true )
{
if ( d_extended_correlation_in_fpga = = true )
{
2020-06-24 00:43:37 +00:00
d_multicorrelator_fpga - > initialize_secondary_code ( 0 , & d_secondary_code_string ) ;
2019-07-19 09:55:53 +00:00
}
}
2019-03-22 18:03:46 +00:00
}
}
2019-10-26 20:24:42 +00:00
std : : fill_n ( d_correlator_outs . begin ( ) , d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2019-03-22 18:03:46 +00:00
d_carrier_lock_fail_counter = 0 ;
2019-07-17 16:01:27 +00:00
d_code_lock_fail_counter = 0 ;
2019-03-22 18:03:46 +00:00
d_rem_code_phase_samples = 0.0 ;
d_rem_carr_phase_rad = 0.0 ;
d_rem_code_phase_chips = 0.0 ;
d_acc_carrier_phase_rad = 0.0 ;
d_cn0_estimation_counter = 0 ;
d_carrier_lock_test = 1.0 ;
d_CN0_SNV_dB_Hz = 0.0 ;
2019-07-19 15:23:00 +00:00
d_code_phase_rate_step_chips = 0.0 ;
2019-03-22 18:03:46 +00:00
if ( d_veml )
{
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 3 ] = d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = d_trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2019-03-22 18:03:46 +00:00
}
else
{
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 2 ] = d_trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2019-03-22 18:03:46 +00:00
}
2019-04-01 10:38:45 +00:00
d_current_correlation_time_s = d_code_period ;
2019-04-11 09:44:00 +00:00
// DLL/PLL filter initialization
2020-06-19 01:22:01 +00:00
d_carrier_loop_filter . set_params ( d_trk_parameters . fll_bw_hz , d_trk_parameters . pll_bw_hz , d_trk_parameters . pll_filter_order ) ;
d_code_loop_filter . set_noise_bandwidth ( d_trk_parameters . dll_bw_hz ) ;
2020-07-10 19:13:17 +00:00
d_code_loop_filter . set_update_interval ( static_cast < float > ( d_code_period ) ) ;
2019-04-11 09:44:00 +00:00
d_code_loop_filter . initialize ( ) ; // initialize the code filter
2019-03-22 18:03:46 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > set_local_code_and_taps ( d_local_code_shift_chips . data ( ) , d_prompt_data_shift , d_acquisition_gnss_synchro - > PRN ) ;
2019-03-22 18:03:46 +00:00
2019-04-02 13:53:16 +00:00
d_pull_in_transitory = true ;
d_cloop = true ;
d_Prompt_circular_buffer . clear ( ) ;
2019-06-18 16:22:01 +00:00
2020-06-19 01:22:01 +00:00
d_T_chip_seconds = 1.0 / d_code_freq_chips ;
d_T_prn_seconds = d_T_chip_seconds * static_cast < double > ( d_code_length_chips ) ;
2019-06-18 16:22:01 +00:00
2019-07-19 09:55:53 +00:00
// re-establish nominal integration length (not extended integration by default)
2020-06-19 01:22:01 +00:00
d_current_integration_length_samples = static_cast < int32_t > ( d_trk_parameters . vector_length ) ;
2019-07-19 09:55:53 +00:00
d_next_integration_length_samples = d_current_integration_length_samples ;
2019-06-18 16:22:01 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > disable_secondary_codes ( ) ; // make sure the processing of the secondary codes is disabled by default
2019-06-20 12:28:47 +00:00
2019-07-19 09:55:53 +00:00
d_current_fpga_integration_period = 1 ;
d_current_extended_correlation_in_fpga = false ;
2019-07-19 15:23:00 +00:00
d_cn0_smoother . reset ( ) ;
d_carrier_lock_test_smoother . reset ( ) ;
2019-03-22 18:03:46 +00:00
}
2018-03-02 16:40:13 +00:00
}
2018-12-14 17:15:21 +00:00
2018-10-28 10:07:53 +00:00
void dll_pll_veml_tracking_fpga : : stop_tracking ( )
{
2019-07-24 09:26:16 +00:00
// interrupt the tracking loops
2019-07-19 09:55:53 +00:00
d_stop_tracking = true ;
2019-12-16 17:44:22 +00:00
// let the samples pass through
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > unlock_channel ( ) ;
2018-10-28 10:07:53 +00:00
}
2018-05-09 09:11:12 +00:00
2019-03-01 09:11:36 +00:00
2019-08-14 00:14:29 +00:00
void dll_pll_veml_tracking_fpga : : reset ( )
2018-05-09 09:11:12 +00:00
{
2019-07-24 09:26:16 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > unlock_channel ( ) ;
2019-03-01 09:11:36 +00:00
}
int dll_pll_veml_tracking_fpga : : general_work ( int noutput_items __attribute__ ( ( unused ) ) ,
gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
gr_vector_const_void_star & input_items __attribute__ ( ( unused ) ) ,
gr_vector_void_star & output_items )
2018-05-09 09:11:12 +00:00
{
2019-07-24 09:26:16 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2018-12-10 18:05:12 +00:00
auto * * out = reinterpret_cast < Gnss_Synchro * * > ( & output_items [ 0 ] ) ;
2018-05-09 09:11:12 +00:00
Gnss_Synchro current_synchro_data = Gnss_Synchro ( ) ;
2019-07-10 16:02:57 +00:00
current_synchro_data . Flag_valid_symbol_output = false ;
2021-10-05 19:18:43 +00:00
bool loss_of_lock = false ;
2018-05-09 09:11:12 +00:00
2019-07-19 09:55:53 +00:00
while ( ( ! current_synchro_data . Flag_valid_symbol_output ) & & ( ! d_stop_tracking ) )
{
d_current_integration_length_samples = d_next_integration_length_samples ;
if ( d_pull_in_transitory = = true )
{
if ( d_sample_counter > 0 ) // do not execute this condition until the sample counter has ben read for the first time after start_tracking
{
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . pull_in_time_s < ( d_sample_counter - d_acq_sample_stamp ) / static_cast < int > ( d_trk_parameters . fs_in ) )
2019-07-19 09:55:53 +00:00
{
d_pull_in_transitory = false ;
d_carrier_lock_fail_counter = 0 ;
d_code_lock_fail_counter = 0 ;
}
}
}
switch ( d_state )
{
case 1 : // Pull-in
{
boost : : mutex : : scoped_lock lock ( d_mutex ) ;
2023-12-21 12:57:41 +00:00
d_worker_is_done = false ;
2019-10-11 17:24:20 +00:00
while ( ! d_worker_is_done )
{
2020-06-19 01:22:01 +00:00
d_m_condition . wait ( lock ) ;
2019-10-11 17:24:20 +00:00
}
2019-07-19 09:55:53 +00:00
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
int64_t acq_trk_diff_samples ;
double acq_trk_diff_seconds ;
double delta_trk_to_acq_prn_start_samples ;
2019-07-19 15:23:00 +00:00
uint64_t absolute_samples_offset ;
2019-07-19 09:55:53 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > lock_channel ( ) ;
2020-07-19 07:39:32 +00:00
const uint64_t counter_value = d_multicorrelator_fpga - > read_sample_counter ( ) ;
2019-07-19 09:55:53 +00:00
if ( counter_value > ( d_acq_sample_stamp + d_acq_code_phase_samples ) )
{
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
acq_trk_diff_samples = static_cast < int64_t > ( counter_value ) - static_cast < int64_t > ( d_acq_sample_stamp ) ;
2020-06-19 01:22:01 +00:00
acq_trk_diff_seconds = static_cast < double > ( acq_trk_diff_samples ) / d_trk_parameters . fs_in ;
2019-07-19 09:55:53 +00:00
delta_trk_to_acq_prn_start_samples = static_cast < double > ( acq_trk_diff_samples ) - d_acq_code_phase_samples ;
2020-07-19 07:39:32 +00:00
const uint32_t num_frames = ceil ( ( delta_trk_to_acq_prn_start_samples ) / d_current_integration_length_samples ) ;
2019-07-19 09:55:53 +00:00
absolute_samples_offset = static_cast < uint64_t > ( d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples ) ;
}
else
{
// test mode
acq_trk_diff_samples = - static_cast < int64_t > ( counter_value ) + static_cast < int64_t > ( d_acq_sample_stamp ) ;
2020-06-19 01:22:01 +00:00
acq_trk_diff_seconds = static_cast < double > ( acq_trk_diff_samples ) / d_trk_parameters . fs_in ;
2019-07-19 09:55:53 +00:00
delta_trk_to_acq_prn_start_samples = static_cast < double > ( acq_trk_diff_samples ) + d_acq_code_phase_samples ;
absolute_samples_offset = static_cast < uint64_t > ( delta_trk_to_acq_prn_start_samples ) ;
}
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > set_initial_sample ( absolute_samples_offset ) ;
2019-07-19 09:55:53 +00:00
d_sample_counter = absolute_samples_offset ;
d_sample_counter_next = d_sample_counter ;
// Doppler effect Fd = (C / (C + Vr)) * F
2020-07-19 07:39:32 +00:00
const double radial_velocity = ( d_signal_carrier_freq + d_acq_carrier_doppler_hz ) / d_signal_carrier_freq ;
2019-07-19 09:55:53 +00:00
// new chip and PRN sequence periods based on acq Doppler
d_code_freq_chips = radial_velocity * d_code_chip_rate ;
2020-06-19 01:22:01 +00:00
d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters . fs_in ;
2019-07-19 09:55:53 +00:00
d_acq_code_phase_samples = absolute_samples_offset ;
2020-07-19 07:39:32 +00:00
const int32_t samples_offset = round ( d_acq_code_phase_samples ) ;
2019-07-19 09:55:53 +00:00
d_acc_carrier_phase_rad - = d_carrier_phase_step_rad * static_cast < double > ( samples_offset ) ;
2019-07-19 15:23:00 +00:00
2019-07-19 09:55:53 +00:00
d_state = 2 ;
LOG ( INFO ) < < " Number of samples between Acquisition and Tracking = " < < acq_trk_diff_samples < < " ( " < < acq_trk_diff_seconds < < " s) " ;
DLOG ( INFO ) < < " PULL-IN Doppler [Hz] = " < < d_carrier_doppler_hz
< < " . PULL-IN Code Phase [samples] = " < < d_acq_code_phase_samples ;
2019-07-22 15:53:26 +00:00
// DEBUG OUTPUT
2020-07-07 16:53:50 +00:00
std : : cout < < " Tracking of " < < d_systemName < < " " < < d_signal_pretty_name < < " signal started on channel " < < d_channel < < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2020-06-19 01:22:01 +00:00
DLOG ( INFO ) < < " Starting tracking of satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < " on channel " < < d_channel ;
2019-07-19 09:55:53 +00:00
2019-07-28 10:01:11 +00:00
// DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)";
2020-07-07 16:53:50 +00:00
// std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)\n";
2019-07-28 10:01:11 +00:00
// DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz
// << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples;
2019-07-19 09:55:53 +00:00
break ;
}
case 2 : // Wide tracking and symbol synchronization
{
d_sample_counter = d_sample_counter_next ;
d_sample_counter_next = d_sample_counter + static_cast < uint64_t > ( d_current_integration_length_samples ) ;
do_correlation_step ( ) ;
// Save single correlation step variables
if ( d_veml )
{
d_VE_accu = * d_Very_Early ;
d_VL_accu = * d_Very_Late ;
}
d_E_accu = * d_Early ;
d_P_accu = * d_Prompt ;
d_L_accu = * d_Late ;
2020-06-19 01:22:01 +00:00
d_trk_parameters . spc = d_trk_parameters . early_late_space_chips ;
// if (std::string(d_trk_parameters.signal) == "E1")
2020-02-13 17:45:57 +00:00
// {
2020-06-19 01:22:01 +00:00
// d_trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc);
// d_trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc);
2020-02-13 17:45:57 +00:00
// }
2019-07-19 09:55:53 +00:00
2019-07-28 10:01:11 +00:00
// fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period
2020-06-19 01:22:01 +00:00
if ( d_trk_parameters . bit_synchronization_time_limit_s < ( d_sample_counter - d_acq_sample_stamp ) / static_cast < int > ( d_trk_parameters . fs_in ) )
2019-07-19 09:55:53 +00:00
{
2019-07-28 10:01:11 +00:00
d_carrier_lock_fail_counter = 300000 ; // force loss-of-lock condition
2020-06-19 01:22:01 +00:00
LOG ( INFO ) < < d_systemName < < " " < < d_signal_pretty_name < < " tracking synchronization time limit reached in channel " < < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2019-07-19 09:55:53 +00:00
}
// Check lock status
if ( ! cn0_and_tracking_lock_status ( d_code_period ) )
{
clear_tracking_vars ( ) ;
2021-10-05 19:18:43 +00:00
d_state = 1 ; // loss-of-lock detected
loss_of_lock = true ; // Set the flag so that the negative indication can be generated
current_synchro_data = * d_acquisition_gnss_synchro ; // Fill in the Gnss_Synchro object with basic info
2019-07-19 09:55:53 +00:00
}
else
{
bool next_state = false ;
// Perform DLL/PLL tracking loop computations. Costas Loop enabled
run_dll_pll ( ) ;
update_tracking_vars ( ) ;
// enable write dump file this cycle (valid DLL/PLL cycle)
log_data ( ) ;
if ( ! d_pull_in_transitory )
{
if ( d_secondary )
{
// ####### SECONDARY CODE LOCK #####
d_Prompt_circular_buffer . push_back ( * d_Prompt ) ;
if ( d_Prompt_circular_buffer . size ( ) = = d_secondary_code_length )
{
next_state = acquire_secondary ( ) ;
if ( next_state )
{
2020-06-19 01:22:01 +00:00
LOG ( INFO ) < < d_systemName < < " " < < d_signal_pretty_name < < " secondary code locked in channel " < < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2020-06-19 01:22:01 +00:00
std : : cout < < d_systemName < < " " < < d_signal_pretty_name < < " secondary code locked in channel " < < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2019-07-19 09:55:53 +00:00
}
}
}
2019-07-28 10:01:11 +00:00
else if ( d_symbols_per_bit > 1 ) // Signal does not have secondary code. Search a bit transition by sign change
2019-07-19 09:55:53 +00:00
{
2019-07-28 10:01:11 +00:00
// ******* preamble correlation ********
2019-07-19 09:55:53 +00:00
d_Prompt_circular_buffer . push_back ( * d_Prompt ) ;
if ( d_Prompt_circular_buffer . size ( ) = = d_secondary_code_length )
{
next_state = acquire_secondary ( ) ;
if ( next_state )
{
2020-06-19 01:22:01 +00:00
LOG ( INFO ) < < d_systemName < < " " < < d_signal_pretty_name < < " tracking bit synchronization locked in channel " < < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2020-06-19 01:22:01 +00:00
std : : cout < < d_systemName < < " " < < d_signal_pretty_name < < " tracking bit synchronization locked in channel " < < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2019-07-19 09:55:53 +00:00
}
}
}
else
{
next_state = true ;
}
}
else
{
2019-07-28 10:01:11 +00:00
next_state = false ; // keep in state 2 during pull-in transitory
2019-07-19 09:55:53 +00:00
}
if ( next_state )
{ // reset extended correlator
d_VE_accu = gr_complex ( 0.0 , 0.0 ) ;
d_E_accu = gr_complex ( 0.0 , 0.0 ) ;
d_P_accu = gr_complex ( 0.0 , 0.0 ) ;
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
d_L_accu = gr_complex ( 0.0 , 0.0 ) ;
d_VL_accu = gr_complex ( 0.0 , 0.0 ) ;
d_Prompt_circular_buffer . clear ( ) ;
d_current_symbol = 0 ;
d_current_data_symbol = 0 ;
if ( d_enable_extended_integration )
{
// update integration time
d_extend_correlation_symbols_count = 0 ;
2020-06-19 01:22:01 +00:00
d_current_correlation_time_s = static_cast < float > ( d_trk_parameters . extend_correlation_symbols ) * static_cast < float > ( d_code_period ) ;
2019-07-19 09:55:53 +00:00
if ( d_extended_correlation_in_fpga )
{
d_current_fpga_integration_period = d_fpga_integration_period ;
d_current_extended_correlation_in_fpga = true ;
if ( d_sc_demodulate_enabled )
{
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > enable_secondary_codes ( ) ;
2019-07-19 09:55:53 +00:00
}
if ( d_extend_fpga_integration_periods > 1 )
{
// correction on already computed parameters
2020-06-19 01:22:01 +00:00
d_K_blk_samples = d_T_prn_samples * ( d_fpga_integration_period ) + d_rem_code_phase_samples_prev ;
d_next_integration_length_samples = static_cast < int32_t > ( std : : floor ( d_K_blk_samples ) ) ;
2019-07-19 09:55:53 +00:00
d_state = 5 ;
}
else
{
// correction on already computed parameters
2020-06-19 01:22:01 +00:00
d_K_blk_samples = d_T_prn_samples * d_trk_parameters . extend_correlation_symbols + d_rem_code_phase_samples_prev ;
d_next_integration_length_samples = static_cast < int32_t > ( std : : floor ( d_K_blk_samples ) ) ;
2019-07-19 09:55:53 +00:00
d_state = 6 ;
}
}
else
{
d_state = 3 ; // next state is the extended correlator integrator
}
2020-06-19 01:22:01 +00:00
LOG ( INFO ) < < " Enabled " < < d_trk_parameters . extend_correlation_symbols * static_cast < int32_t > ( d_code_period * 1000.0 ) < < " ms extended correlator in channel "
2019-07-19 09:55:53 +00:00
< < d_channel
2020-06-19 01:22:01 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) ;
std : : cout < < " Enabled " < < d_trk_parameters . extend_correlation_symbols * static_cast < int32_t > ( d_code_period * 1000.0 ) < < " ms extended correlator in channel "
2019-07-19 09:55:53 +00:00
< < d_channel
2020-07-07 16:53:50 +00:00
< < " for satellite " < < Gnss_Satellite ( d_systemName , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2019-07-19 09:55:53 +00:00
// Set narrow taps delay values [chips]
2020-07-10 19:13:17 +00:00
d_code_loop_filter . set_update_interval ( static_cast < float > ( d_current_correlation_time_s ) ) ;
2020-06-19 01:22:01 +00:00
d_code_loop_filter . set_noise_bandwidth ( d_trk_parameters . dll_bw_narrow_hz ) ;
d_carrier_loop_filter . set_params ( d_trk_parameters . fll_bw_hz , d_trk_parameters . pll_bw_narrow_hz , d_trk_parameters . pll_filter_order ) ;
2019-07-19 09:55:53 +00:00
if ( d_veml )
{
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . very_early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - d_trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 3 ] = d_trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = d_trk_parameters . very_early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_narrow_chips ;
// d_trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc);
// d_trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc);
2019-07-19 09:55:53 +00:00
}
else
{
2020-06-19 01:22:01 +00:00
d_local_code_shift_chips [ 0 ] = - d_trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 2 ] = d_trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_trk_parameters . spc = d_trk_parameters . early_late_space_narrow_chips ;
2019-07-19 09:55:53 +00:00
}
}
else
{
d_state = 4 ;
}
}
}
break ;
}
case 3 : // coherent integration (correlation time extension)
{
d_sample_counter = d_sample_counter_next ;
d_sample_counter_next = d_sample_counter + static_cast < uint64_t > ( d_current_integration_length_samples ) ;
// perform a correlation step
do_correlation_step ( ) ;
save_correlation_results ( ) ;
update_tracking_vars ( ) ;
if ( d_current_data_symbol = = 0 )
{
log_data ( ) ;
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2020-06-19 01:22:01 +00:00
if ( d_interchange_iq )
2019-09-19 16:07:13 +00:00
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
}
2019-07-19 09:55:53 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
current_synchro_data . Flag_valid_symbol_output = true ;
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
}
d_extend_correlation_symbols_count + + ;
2020-06-19 01:22:01 +00:00
if ( d_extend_correlation_symbols_count = = ( d_trk_parameters . extend_correlation_symbols - 1 ) )
2019-07-19 09:55:53 +00:00
{
d_extend_correlation_symbols_count = 0 ;
d_state = 4 ;
}
break ;
}
case 4 : // narrow tracking
{
d_sample_counter = d_sample_counter_next ;
d_sample_counter_next = d_sample_counter + static_cast < uint64_t > ( d_current_integration_length_samples ) ;
// perform a correlation step
do_correlation_step ( ) ;
save_correlation_results ( ) ;
// check lock status
2020-06-19 01:22:01 +00:00
if ( ! cn0_and_tracking_lock_status ( d_code_period * static_cast < double > ( d_trk_parameters . extend_correlation_symbols ) ) )
2019-07-19 09:55:53 +00:00
{
clear_tracking_vars ( ) ;
2021-10-05 19:18:43 +00:00
d_state = 1 ; // loss-of-lock detected
loss_of_lock = true ; // Set the flag so that the negative indication can be generated
current_synchro_data = * d_acquisition_gnss_synchro ; // Fill in the Gnss_Synchro object with basic info
2019-07-19 09:55:53 +00:00
}
else
{
run_dll_pll ( ) ;
update_tracking_vars ( ) ;
2020-03-08 11:00:03 +00:00
check_carrier_phase_coherent_initialization ( ) ;
2019-07-19 09:55:53 +00:00
if ( d_current_data_symbol = = 0 )
{
// enable write dump file this cycle (valid DLL/PLL cycle)
log_data ( ) ;
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2020-06-19 01:22:01 +00:00
if ( d_interchange_iq )
2019-09-19 16:07:13 +00:00
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
}
2019-07-19 09:55:53 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
current_synchro_data . Flag_valid_symbol_output = true ;
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
}
// reset extended correlator
d_VE_accu = gr_complex ( 0.0 , 0.0 ) ;
d_E_accu = gr_complex ( 0.0 , 0.0 ) ;
d_P_accu = gr_complex ( 0.0 , 0.0 ) ;
d_L_accu = gr_complex ( 0.0 , 0.0 ) ;
d_VL_accu = gr_complex ( 0.0 , 0.0 ) ;
if ( d_enable_extended_integration )
{
d_state = 3 ; // new coherent integration (correlation time extension) cycle
}
}
break ;
}
case 5 : // coherent integration (correlation time extension)
{
d_sample_counter = d_sample_counter_next ;
d_sample_counter_next = d_sample_counter + static_cast < uint64_t > ( d_current_integration_length_samples ) ;
// this must be computed for the secondary prn code
if ( d_secondary )
{
2020-07-19 07:39:32 +00:00
const uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period ;
const uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * ( d_fpga_integration_period - 1 ) ;
2019-07-19 09:55:53 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > update_prn_code_length ( first_prn_length , next_prn_length ) ;
2019-07-19 09:55:53 +00:00
}
// perform a correlation step
do_correlation_step ( ) ;
save_correlation_results ( ) ;
update_tracking_vars ( ) ;
if ( d_current_data_symbol = = 0 )
{
log_data ( ) ;
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2020-06-19 01:22:01 +00:00
if ( d_interchange_iq )
2019-09-19 16:07:13 +00:00
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
}
2019-07-19 09:55:53 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
current_synchro_data . Flag_valid_symbol_output = true ;
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
}
d_extend_correlation_symbols_count + + ;
if ( d_extend_correlation_symbols_count = = ( d_extend_fpga_integration_periods - 1 ) )
{
d_extend_correlation_symbols_count = 0 ;
d_state = 6 ;
}
break ;
}
case 6 : // narrow tracking IN THE FPGA
{
d_sample_counter = d_sample_counter_next ;
d_sample_counter_next = d_sample_counter + static_cast < uint64_t > ( d_current_integration_length_samples ) ;
// this must be computed for the secondary prn code
if ( d_secondary )
{
2020-07-19 07:39:32 +00:00
const uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period ;
const uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * ( d_fpga_integration_period - 1 ) ;
2019-07-19 09:55:53 +00:00
2020-06-19 01:22:01 +00:00
d_multicorrelator_fpga - > update_prn_code_length ( first_prn_length , next_prn_length ) ;
2019-07-19 09:55:53 +00:00
}
// perform a correlation step
do_correlation_step ( ) ;
save_correlation_results ( ) ;
// check lock status
2020-06-19 01:22:01 +00:00
if ( ! cn0_and_tracking_lock_status ( d_code_period * static_cast < double > ( d_trk_parameters . extend_correlation_symbols ) ) )
2019-07-19 09:55:53 +00:00
{
clear_tracking_vars ( ) ;
2021-10-05 19:18:43 +00:00
d_state = 1 ; // loss-of-lock detected
loss_of_lock = true ; // Set the flag so that the negative indication can be generated
current_synchro_data = * d_acquisition_gnss_synchro ; // Fill in the Gnss_Synchro object with basic info
2019-07-19 09:55:53 +00:00
}
else
{
run_dll_pll ( ) ;
update_tracking_vars ( ) ;
2021-08-03 08:18:27 +00:00
check_carrier_phase_coherent_initialization ( ) ;
2019-07-19 09:55:53 +00:00
if ( d_current_data_symbol = = 0 )
{
// enable write dump file this cycle (valid DLL/PLL cycle)
log_data ( ) ;
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2020-06-19 01:22:01 +00:00
if ( d_interchange_iq )
2019-09-19 16:07:13 +00:00
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( d_P_data_accu . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( d_P_data_accu . imag ( ) ) ;
}
2019-07-19 09:55:53 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
current_synchro_data . Flag_valid_symbol_output = true ;
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
}
d_extend_correlation_symbols_count = 0 ;
// reset extended correlator
d_VE_accu = gr_complex ( 0.0 , 0.0 ) ;
d_E_accu = gr_complex ( 0.0 , 0.0 ) ;
d_P_accu = gr_complex ( 0.0 , 0.0 ) ;
d_L_accu = gr_complex ( 0.0 , 0.0 ) ;
d_VL_accu = gr_complex ( 0.0 , 0.0 ) ;
if ( d_extend_fpga_integration_periods > 1 )
{
d_state = 5 ;
}
}
break ;
}
2019-07-28 10:01:11 +00:00
default :
break ;
2019-07-19 09:55:53 +00:00
}
}
2019-06-18 16:22:01 +00:00
2021-10-05 19:18:43 +00:00
if ( current_synchro_data . Flag_valid_symbol_output | | loss_of_lock )
2019-07-17 16:01:27 +00:00
{
2020-06-19 01:22:01 +00:00
current_synchro_data . fs = static_cast < int64_t > ( d_trk_parameters . fs_in ) ;
2019-07-28 10:01:11 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter_next ; // d_sample_counter;
2021-10-05 19:18:43 +00:00
current_synchro_data . Flag_valid_symbol_output = ! loss_of_lock ;
2021-10-05 19:38:32 +00:00
current_synchro_data . Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked ;
2023-11-27 12:41:55 +00:00
* out [ 0 ] = std : : move ( current_synchro_data ) ;
2019-07-17 16:01:27 +00:00
return 1 ;
2018-05-09 09:11:12 +00:00
}
return 0 ;
}