2018-02-12 16:17:39 +00:00
/*!
* \ file dll_pll_veml_tracking . cc
2018-03-28 17:54:24 +00:00
* \ brief Implementation of a code DLL + carrier PLL tracking block .
2018-11-29 16:09:30 +00:00
* \ author Javier Arribas , 2018. jarribas ( at ) cttc . es
2018-03-29 07:23:23 +00:00
* \ author Antonio Ramos , 2018 antonio . ramosdet ( at ) gmail . com
2018-02-12 16:17:39 +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
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2018-03-28 17:54:24 +00:00
* Copyright ( C ) 2010 - 2018 ( see AUTHORS file for a list of contributors )
2018-02-12 16:17:39 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
* GNSS - SDR is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
* ( at your option ) any later version .
*
* GNSS - SDR is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
2018-05-13 20:49:11 +00:00
* along with GNSS - SDR . If not , see < https : //www.gnu.org/licenses/>.
2018-02-12 16:17:39 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
# include "dll_pll_veml_tracking.h"
2019-01-28 01:29:43 +00:00
# include "Beidou_B1I.h"
2018-02-12 16:17:39 +00:00
# include "GPS_L1_CA.h"
# include "GPS_L2C.h"
# include "GPS_L5.h"
# include "Galileo_E1.h"
# include "Galileo_E5a.h"
2018-12-09 21:00:09 +00:00
# include "MATH_CONSTANTS.h"
2019-01-28 01:29:43 +00:00
# include "beidou_b1i_signal_processing.h"
2018-12-09 21:00:09 +00:00
# include "galileo_e1_signal_processing.h"
2018-03-13 11:38:33 +00:00
# include "galileo_e5_signal_processing.h"
2018-10-29 23:47:12 +00:00
# include "gnss_sdr_create_directory.h"
2018-03-13 11:38:33 +00:00
# include "gps_l2c_signal.h"
# include "gps_l5_signal.h"
2018-12-09 21:00:09 +00:00
# include "gps_sdr_signal_processing.h"
# include "lock_detectors.h"
# include "tracking_discriminators.h"
2018-11-04 21:50:46 +00:00
# include <boost/filesystem/path.hpp>
2018-03-13 11:38:33 +00:00
# include <glog/logging.h>
2018-03-28 17:54:24 +00:00
# include <gnuradio/io_signature.h>
2018-03-13 11:38:33 +00:00
# include <matio.h>
# include <volk_gnsssdr/volk_gnsssdr.h>
2018-03-28 17:54:24 +00:00
# include <algorithm>
# include <cmath>
2019-02-10 20:55:51 +00:00
# include <exception>
2018-03-28 17:54:24 +00:00
# include <iostream>
2018-09-06 22:06:06 +00:00
# include <numeric>
2018-12-09 21:00:09 +00:00
# include <sstream>
2018-02-12 16:17:39 +00:00
2018-06-19 10:56:53 +00:00
dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking ( const Dll_Pll_Conf & conf_ )
2018-02-12 16:17:39 +00:00
{
2018-04-05 16:08:49 +00:00
return dll_pll_veml_tracking_sptr ( new dll_pll_veml_tracking ( conf_ ) ) ;
2018-02-12 16:17:39 +00:00
}
2018-03-12 14:16:39 +00:00
void dll_pll_veml_tracking : : forecast ( int noutput_items ,
gr_vector_int & ninput_items_required )
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
if ( noutput_items ! = 0 )
{
2018-08-12 22:54:23 +00:00
ninput_items_required [ 0 ] = static_cast < int32_t > ( trk_parameters . vector_length ) * 2 ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
2018-06-19 10:56:53 +00:00
dll_pll_veml_tracking : : dll_pll_veml_tracking ( const Dll_Pll_Conf & conf_ ) : gr : : block ( " dll_pll_veml_tracking " , gr : : io_signature : : make ( 1 , 1 , sizeof ( gr_complex ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2018-02-12 16:17:39 +00:00
{
2018-04-06 08:48:10 +00:00
trk_parameters = conf_ ;
2018-02-12 16:17:39 +00:00
// Telemetry bit synchronization message port input
this - > message_port_register_out ( pmt : : mp ( " events " ) ) ;
2018-04-05 16:08:49 +00:00
this - > set_relative_rate ( 1.0 / static_cast < double > ( trk_parameters . vector_length ) ) ;
2018-02-12 16:17:39 +00:00
2018-07-13 13:23:20 +00:00
// Telemetry bit synchronization message port input (mainly for GPS L1 CA)
this - > message_port_register_in ( pmt : : mp ( " preamble_samplestamp " ) ) ;
2018-02-12 16:17:39 +00:00
// initialize internal vars
2018-03-13 11:38:33 +00:00
d_veml = false ;
2018-03-19 10:19:55 +00:00
d_cloop = true ;
2018-02-12 16:17:39 +00:00
d_code_chip_rate = 0.0 ;
2018-08-12 22:54:23 +00:00
d_secondary_code_length = 0U ;
2018-03-12 14:16:39 +00:00
d_secondary_code_string = nullptr ;
2018-12-24 21:14:10 +00:00
d_preambles_symbols = nullptr ;
2019-02-14 21:49:36 +00:00
d_preamble_length_symbols = 0 ;
2018-04-05 16:08:49 +00:00
signal_type = std : : string ( trk_parameters . signal ) ;
2018-04-28 10:33:46 +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 " ;
2018-07-10 21:30:15 +00:00
map_signal_pretty_name [ " B1 " ] = " B1I " ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = map_signal_pretty_name [ signal_type ] ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . system = = ' G ' )
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
systemName = " GPS " ;
2018-12-03 11:52:17 +00:00
if ( signal_type = = " 1C " )
2018-03-12 14:16:39 +00:00
{
d_signal_carrier_freq = GPS_L1_FREQ_HZ ;
d_code_period = GPS_L1_CA_CODE_PERIOD ;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ ;
2018-03-13 11:38:33 +00:00
d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT ;
2018-03-12 14:16:39 +00:00
d_correlation_length_ms = 1 ;
2018-03-13 11:38:33 +00:00
d_code_samples_per_chip = 1 ;
2018-08-11 10:56:52 +00:00
d_code_length_chips = static_cast < uint32_t > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) ;
2018-03-29 07:23:23 +00:00
// GPS L1 C/A does not have pilot component nor secondary code
2018-03-12 14:16:39 +00:00
d_secondary = false ;
2018-04-05 16:08:49 +00:00
trk_parameters . track_pilot = false ;
2018-03-13 11:38:33 +00:00
interchange_iq = false ;
2018-07-13 13:23:20 +00:00
// set the preamble
2018-08-11 10:56:52 +00:00
uint16_t preambles_bits [ GPS_CA_PREAMBLE_LENGTH_BITS ] = GPS_PREAMBLE ;
2018-07-13 13:23:20 +00:00
// preamble bits to sampled symbols
2018-12-24 21:14:10 +00:00
d_preamble_length_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS ;
d_preambles_symbols = static_cast < int32_t * > ( volk_gnsssdr_malloc ( GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof ( int32_t ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-08-11 10:56:52 +00:00
int32_t n = 0 ;
2018-12-08 17:49:31 +00:00
for ( uint16_t preambles_bit : preambles_bits )
2018-07-13 13:23:20 +00:00
{
2018-08-11 10:56:52 +00:00
for ( uint32_t j = 0 ; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT ; j + + )
2018-07-13 13:23:20 +00:00
{
2018-12-03 17:03:25 +00:00
if ( preambles_bit = = 1 )
2018-07-13 13:23:20 +00:00
{
2018-12-24 21:14:10 +00:00
d_preambles_symbols [ n ] = 1 ;
2018-07-13 13:23:20 +00:00
}
else
{
2018-12-24 21:14:10 +00:00
d_preambles_symbols [ n ] = - 1 ;
2018-07-13 13:23:20 +00:00
}
n + + ;
}
}
2019-02-28 13:10:44 +00:00
d_symbol_history . set_capacity ( GPS_CA_PREAMBLE_LENGTH_SYMBOLS ) ; // Change fixed buffer size
d_symbol_history . clear ( ) ; // Clear all the elements in the buffer
2018-03-12 14:16:39 +00:00
}
2018-12-03 11:52:17 +00:00
else if ( signal_type = = " 2S " )
2018-03-12 14:16:39 +00:00
{
d_signal_carrier_freq = GPS_L2_FREQ_HZ ;
d_code_period = GPS_L2_M_PERIOD ;
d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ ;
2018-08-11 10:56:52 +00:00
d_code_length_chips = static_cast < uint32_t > ( GPS_L2_M_CODE_LENGTH_CHIPS ) ;
2018-03-13 11:38:33 +00:00
d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL ;
2018-03-12 14:16:39 +00:00
d_correlation_length_ms = 20 ;
2018-03-13 11:38:33 +00:00
d_code_samples_per_chip = 1 ;
2018-03-29 07:23:23 +00:00
// GPS L2 does not have pilot component nor secondary code
2018-03-12 14:16:39 +00:00
d_secondary = false ;
2018-04-05 16:08:49 +00:00
trk_parameters . track_pilot = false ;
2018-03-13 11:38:33 +00:00
interchange_iq = false ;
2018-03-12 14:16:39 +00:00
}
2018-12-03 11:52:17 +00:00
else if ( signal_type = = " L5 " )
2018-03-12 14:16:39 +00:00
{
d_signal_carrier_freq = GPS_L5_FREQ_HZ ;
2019-02-22 09:47:24 +00:00
d_code_period = GPS_L5I_PERIOD ;
d_code_chip_rate = GPS_L5I_CODE_RATE_HZ ;
2018-03-13 11:38:33 +00:00
d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL ;
2018-03-12 14:16:39 +00:00
d_correlation_length_ms = 1 ;
2018-03-13 11:38:33 +00:00
d_code_samples_per_chip = 1 ;
2019-02-22 09:47:24 +00:00
d_code_length_chips = static_cast < uint32_t > ( GPS_L5I_CODE_LENGTH_CHIPS ) ;
2018-03-13 11:38:33 +00:00
d_secondary = true ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-13 11:38:33 +00:00
{
2019-02-22 09:47:24 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GPS_L5Q_NH_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & GPS_L5Q_NH_CODE_STR ) ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " Q " ;
2018-05-04 11:39:45 +00:00
interchange_iq = true ;
2018-03-13 11:38:33 +00:00
}
else
{
2019-02-22 09:47:24 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GPS_L5I_NH_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & GPS_L5I_NH_CODE_STR ) ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " I " ;
2018-05-04 11:39:45 +00:00
interchange_iq = false ;
2018-03-13 11:38:33 +00:00
}
2018-03-12 14:16:39 +00:00
}
else
{
LOG ( WARNING ) < < " Invalid Signal argument when instantiating tracking blocks " ;
2018-03-29 07:23:23 +00:00
std : : cerr < < " Invalid Signal argument when instantiating tracking blocks " < < std : : endl ;
2018-03-29 13:08:54 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
interchange_iq = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
2018-08-12 22:54:23 +00:00
d_code_length_chips = 0U ;
d_code_samples_per_chip = 0U ;
2018-03-29 13:08:54 +00:00
d_symbols_per_bit = 0 ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
2018-04-05 16:08:49 +00:00
else if ( trk_parameters . system = = ' E ' )
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
systemName = " Galileo " ;
2018-12-03 11:52:17 +00:00
if ( signal_type = = " 1B " )
2018-03-12 14:16:39 +00:00
{
2019-02-22 09:47:24 +00:00
d_signal_carrier_freq = GALILEO_E1_FREQ_HZ ;
d_code_period = GALILEO_E1_CODE_PERIOD ;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ ;
d_code_length_chips = static_cast < uint32_t > ( GALILEO_E1_B_CODE_LENGTH_CHIPS ) ;
2018-03-13 11:38:33 +00:00
d_symbols_per_bit = 1 ;
2018-03-12 14:16:39 +00:00
d_correlation_length_ms = 4 ;
2018-03-13 11:38:33 +00:00
d_code_samples_per_chip = 2 ; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip
d_veml = true ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-16 11:21:13 +00:00
{
d_secondary = true ;
2019-02-22 09:47:24 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E1_C_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & GALILEO_E1_C_SECONDARY_CODE ) ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " C " ;
2018-03-16 11:21:13 +00:00
}
else
{
d_secondary = false ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " B " ;
2018-03-16 11:21:13 +00:00
}
2018-03-13 11:38:33 +00:00
interchange_iq = false ; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD.
2018-03-12 14:16:39 +00:00
}
2018-12-03 11:52:17 +00:00
else if ( signal_type = = " 5X " )
2018-03-12 14:16:39 +00:00
{
2019-02-22 09:47:24 +00:00
d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ ;
d_code_period = GALILEO_E5A_CODE_PERIOD ;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ ;
2018-03-13 11:38:33 +00:00
d_symbols_per_bit = 20 ;
2018-03-12 14:16:39 +00:00
d_correlation_length_ms = 1 ;
2018-03-13 11:38:33 +00:00
d_code_samples_per_chip = 1 ;
2019-02-22 09:47:24 +00:00
d_code_length_chips = static_cast < uint32_t > ( GALILEO_E5A_CODE_LENGTH_CHIPS ) ;
2018-08-27 16:05:19 +00:00
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-12 14:16:39 +00:00
{
2018-08-27 16:05:19 +00:00
d_secondary = true ;
2019-02-22 09:47:24 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_Q_SECONDARY_CODE_LENGTH ) ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " Q " ;
2018-05-04 11:39:45 +00:00
interchange_iq = true ;
2018-03-12 14:16:39 +00:00
}
else
{
2018-08-27 16:05:19 +00:00
//Do not acquire secondary code in data component. It is done in telemetry decoder
d_secondary = false ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " I " ;
2018-05-04 11:39:45 +00:00
interchange_iq = false ;
2018-03-12 14:16:39 +00:00
}
}
else
{
LOG ( WARNING ) < < " Invalid Signal argument when instantiating tracking blocks " ;
std : : cout < < " Invalid Signal argument when instantiating tracking blocks " < < std : : endl ;
2018-03-29 13:08:54 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
interchange_iq = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
2018-08-12 22:54:23 +00:00
d_code_length_chips = 0U ;
d_code_samples_per_chip = 0U ;
2018-03-29 13:08:54 +00:00
d_symbols_per_bit = 0 ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
2018-07-10 21:30:15 +00:00
else if ( trk_parameters . system = = ' C ' )
{
systemName = " Beidou " ;
2018-12-18 21:55:36 +00:00
if ( signal_type = = " B1 " )
2018-07-10 21:30:15 +00:00
{
2019-01-28 01:29:43 +00:00
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B1I_FREQ_HZ ;
d_code_period = BEIDOU_B1I_CODE_PERIOD ;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ ;
d_code_length_chips = static_cast < unsigned int > ( BEIDOU_B1I_CODE_LENGTH_CHIPS ) ;
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT ;
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
d_secondary = true ;
trk_parameters . track_pilot = false ;
interchange_iq = false ;
d_secondary_code_length = static_cast < unsigned int > ( BEIDOU_B1I_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_SECONDARY_CODE_STR ) ;
2018-07-10 21:30:15 +00:00
}
else
{
LOG ( WARNING ) < < " Invalid Signal argument when instantiating tracking blocks " ;
std : : cout < < " Invalid Signal argument when instantiating tracking blocks " < < std : : endl ;
d_correlation_length_ms = 1 ;
d_secondary = false ;
interchange_iq = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
d_code_length_chips = 0 ;
d_code_samples_per_chip = 0 ;
d_symbols_per_bit = 0 ;
}
}
2018-03-12 14:16:39 +00:00
else
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
LOG ( WARNING ) < < " Invalid System argument when instantiating tracking blocks " ;
2018-03-29 07:23:23 +00:00
std : : cerr < < " Invalid System argument when instantiating tracking blocks " < < std : : endl ;
2018-03-27 09:01:23 +00:00
d_correlation_length_ms = 1 ;
d_secondary = false ;
interchange_iq = false ;
d_signal_carrier_freq = 0.0 ;
d_code_period = 0.0 ;
2018-08-12 22:54:23 +00:00
d_code_length_chips = 0U ;
d_code_samples_per_chip = 0U ;
2018-03-27 09:01:23 +00:00
d_symbols_per_bit = 0 ;
2018-02-12 16:17:39 +00:00
}
2018-03-13 11:38:33 +00:00
T_chip_seconds = 0.0 ;
T_prn_seconds = 0.0 ;
T_prn_samples = 0.0 ;
K_blk_samples = 0.0 ;
2018-02-12 16:17:39 +00:00
// Initialize tracking ==========================================
2018-03-13 11:38:33 +00:00
d_code_loop_filter = Tracking_2nd_DLL_filter ( static_cast < float > ( d_code_period ) ) ;
d_carrier_loop_filter = Tracking_2nd_PLL_filter ( static_cast < float > ( d_code_period ) ) ;
2018-12-03 16:07:51 +00:00
d_code_loop_filter . set_DLL_BW ( trk_parameters . dll_bw_hz ) ;
d_carrier_loop_filter . set_PLL_BW ( trk_parameters . pll_bw_hz ) ;
2018-02-12 16:17:39 +00:00
// Initialization of local code replica
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
2018-03-12 14:16:39 +00:00
d_tracking_code = static_cast < float * > ( volk_gnsssdr_malloc ( 2 * d_code_length_chips * sizeof ( float ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-02-12 16:17:39 +00:00
// correlator outputs (scalar)
2018-03-12 14:16:39 +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 ;
}
2018-02-13 14:20:30 +00:00
2018-03-12 14:16:39 +00:00
d_correlator_outs = static_cast < gr_complex * > ( volk_gnsssdr_malloc ( d_n_correlator_taps * sizeof ( gr_complex ) , volk_gnsssdr_get_alignment ( ) ) ) ;
d_local_code_shift_chips = static_cast < float * > ( volk_gnsssdr_malloc ( d_n_correlator_taps * sizeof ( float ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-06-16 19:03:34 +00:00
2018-03-12 14:16:39 +00:00
// map memory pointers of correlator outputs
if ( d_veml )
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
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 ] ;
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-12 14:16:39 +00:00
d_local_code_shift_chips [ 2 ] = 0.0 ;
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 3 ] = trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-13 11:38:33 +00:00
d_prompt_data_shift = & d_local_code_shift_chips [ 2 ] ;
2018-02-12 16:17:39 +00:00
}
2018-02-13 14:20:30 +00:00
else
2018-03-12 14:16:39 +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 ;
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-12 14:16:39 +00:00
d_local_code_shift_chips [ 1 ] = 0.0 ;
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 2 ] = trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-13 11:38:33 +00:00
d_prompt_data_shift = & d_local_code_shift_chips [ 1 ] ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
2018-04-05 16:08:49 +00:00
multicorrelator_cpu . init ( 2 * trk_parameters . vector_length , d_n_correlator_taps ) ;
2018-03-13 11:38:33 +00:00
2018-04-05 16:08:49 +00:00
if ( trk_parameters . extend_correlation_symbols > 1 )
2018-03-13 11:38:33 +00:00
{
d_enable_extended_integration = true ;
}
else
{
d_enable_extended_integration = false ;
2018-04-05 16:08:49 +00:00
trk_parameters . extend_correlation_symbols = 1 ;
2018-03-13 11:38:33 +00:00
}
2018-02-12 16:17:39 +00:00
// Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-02-12 16:17:39 +00:00
{
// Extra correlator for the data component
2018-04-05 16:08:49 +00:00
correlator_data_cpu . init ( 2 * trk_parameters . vector_length , 1 ) ;
2018-09-09 11:40:31 +00:00
correlator_data_cpu . set_high_dynamics_resampler ( trk_parameters . high_dyn ) ;
2018-03-12 14:16:39 +00:00
d_data_code = static_cast < float * > ( volk_gnsssdr_malloc ( 2 * d_code_length_chips * sizeof ( float ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-02-12 16:17:39 +00:00
}
2018-03-27 09:01:23 +00:00
else
{
d_data_code = nullptr ;
}
2018-02-12 16:17:39 +00:00
2018-05-19 18:50:48 +00:00
// --- Initializations ---
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
2018-09-09 11:40:31 +00:00
multicorrelator_cpu . set_high_dynamics_resampler ( trk_parameters . high_dyn ) ;
2018-02-12 16:17:39 +00:00
// Initial code frequency basis of NCO
2018-02-13 14:20:30 +00:00
d_code_freq_chips = d_code_chip_rate ;
2018-02-12 16:17:39 +00:00
// Residual code phase (in chips)
d_rem_code_phase_samples = 0.0 ;
// Residual carrier phase
d_rem_carr_phase_rad = 0.0 ;
// sample synchronization
2018-08-12 22:54:23 +00:00
d_sample_counter = 0ULL ;
d_acq_sample_stamp = 0ULL ;
2018-02-12 16:17:39 +00:00
2018-08-12 22:54:23 +00:00
d_current_prn_length_samples = static_cast < int32_t > ( trk_parameters . vector_length ) ;
2018-02-12 16:17:39 +00:00
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0 ;
2018-04-28 10:33:46 +00:00
d_Prompt_buffer = new gr_complex [ trk_parameters . cn0_samples ] ;
2018-02-13 14:20:30 +00:00
d_carrier_lock_test = 1.0 ;
d_CN0_SNV_dB_Hz = 0.0 ;
2018-02-12 16:17:39 +00:00
d_carrier_lock_fail_counter = 0 ;
2018-04-28 10:33:46 +00:00
d_carrier_lock_threshold = trk_parameters . carrier_lock_th ;
2018-05-19 21:54:36 +00:00
d_Prompt_Data = static_cast < gr_complex * > ( volk_gnsssdr_malloc ( sizeof ( gr_complex ) , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-02-12 16:17:39 +00:00
2018-02-13 14:20:30 +00:00
d_acquisition_gnss_synchro = nullptr ;
2018-02-12 16:17:39 +00:00
d_channel = 0 ;
d_acq_code_phase_samples = 0.0 ;
d_acq_carrier_doppler_hz = 0.0 ;
d_carrier_doppler_hz = 0.0 ;
d_acc_carrier_phase_rad = 0.0 ;
d_extend_correlation_symbols_count = 0 ;
d_code_phase_step_chips = 0.0 ;
2018-08-15 11:38:49 +00:00
d_code_phase_rate_step_chips = 0.0 ;
2018-02-12 16:17:39 +00:00
d_carrier_phase_step_rad = 0.0 ;
2018-08-28 20:26:26 +00:00
d_carrier_phase_rate_step_rad = 0.0 ;
2018-02-12 16:17:39 +00:00
d_rem_code_phase_chips = 0.0 ;
2018-03-13 11:38:33 +00:00
d_last_prompt = gr_complex ( 0.0 , 0.0 ) ;
2018-03-12 14:16:39 +00:00
d_state = 0 ; // initial state: standby
2018-06-16 19:03:34 +00:00
clear_tracking_vars ( ) ;
2018-09-06 22:06:06 +00:00
if ( trk_parameters . smoother_length > 0 )
{
2019-02-28 13:10:44 +00:00
d_carr_ph_history . set_capacity ( trk_parameters . smoother_length * 2 ) ;
d_code_ph_history . set_capacity ( trk_parameters . smoother_length * 2 ) ;
2018-09-06 22:06:06 +00:00
}
else
{
2019-02-28 13:10:44 +00:00
d_carr_ph_history . set_capacity ( 1 ) ;
d_code_ph_history . set_capacity ( 1 ) ;
2018-09-06 22:06:06 +00:00
}
2018-10-29 23:47:12 +00:00
d_dump = trk_parameters . dump ;
d_dump_mat = trk_parameters . dump_mat and d_dump ;
if ( d_dump )
{
d_dump_filename = trk_parameters . dump_filename ;
std : : string dump_path ;
// Get path
2018-12-03 17:17:21 +00:00
if ( d_dump_filename . find_last_of ( ' / ' ) ! = std : : string : : npos )
2018-10-29 23:47:12 +00:00
{
2018-12-03 17:17:21 +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 ( ' / ' ) ) ;
2018-10-29 23:47:12 +00:00
d_dump_filename = dump_filename_ ;
}
else
{
dump_path = std : : string ( " . " ) ;
}
if ( d_dump_filename . empty ( ) )
{
d_dump_filename = " trk_channel_ " ;
}
// remove extension if any
2018-12-03 17:17:21 +00:00
if ( d_dump_filename . substr ( 1 ) . find_last_of ( ' . ' ) ! = std : : string : : npos )
2018-10-29 23:47:12 +00:00
{
2018-12-03 17:17:21 +00:00
d_dump_filename = d_dump_filename . substr ( 0 , d_dump_filename . find_last_of ( ' . ' ) ) ;
2018-10-29 23:47:12 +00:00
}
d_dump_filename = dump_path + boost : : filesystem : : path : : preferred_separator + d_dump_filename ;
// create directory
if ( ! gnss_sdr_create_directory ( dump_path ) )
{
std : : cerr < < " GNSS-SDR cannot create dump files for the tracking block. Wrong permissions? " < < std : : endl ;
d_dump = false ;
}
}
2018-02-12 16:17:39 +00:00
}
void dll_pll_veml_tracking : : start_tracking ( )
{
2018-02-13 14:20:30 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2018-05-19 18:50:48 +00:00
// correct the code phase according to the delay between acq and trk
2018-02-12 16:17:39 +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 ;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz ;
2018-04-05 16:08:49 +00:00
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters . fs_in ;
2018-08-28 20:26:26 +00:00
d_carrier_phase_rate_step_rad = 0.0 ;
2018-09-10 19:29:44 +00:00
d_carr_ph_history . clear ( ) ;
d_code_ph_history . clear ( ) ;
2018-02-12 16:17:39 +00:00
// DLL/PLL filter initialization
2018-03-12 14:16:39 +00:00
d_carrier_loop_filter . initialize ( ) ; // initialize the carrier filter
d_code_loop_filter . initialize ( ) ; // initialize the code filter
2018-02-12 16:17:39 +00:00
2018-12-03 11:52:17 +00:00
if ( systemName = = " GPS " and signal_type = = " 1C " )
2018-02-12 16:17:39 +00:00
{
2018-03-13 11:38:33 +00:00
gps_l1_ca_code_gen_float ( d_tracking_code , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2018-02-12 16:17:39 +00:00
}
2018-12-03 11:52:17 +00:00
else if ( systemName = = " GPS " and signal_type = = " 2S " )
2018-03-13 11:38:33 +00:00
{
gps_l2c_m_code_gen_float ( d_tracking_code , d_acquisition_gnss_synchro - > PRN ) ;
}
2018-12-03 11:52:17 +00:00
else if ( systemName = = " GPS " and signal_type = = " L5 " )
2018-02-12 16:17:39 +00:00
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-13 11:38:33 +00:00
{
gps_l5q_code_gen_float ( d_tracking_code , d_acquisition_gnss_synchro - > PRN ) ;
gps_l5i_code_gen_float ( d_data_code , d_acquisition_gnss_synchro - > PRN ) ;
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
correlator_data_cpu . set_local_code_and_taps ( d_code_length_chips , d_data_code , d_prompt_data_shift ) ;
}
else
{
gps_l5i_code_gen_float ( d_tracking_code , d_acquisition_gnss_synchro - > PRN ) ;
}
}
2018-12-03 11:52:17 +00:00
else if ( systemName = = " Galileo " and signal_type = = " 1B " )
2018-03-13 11:38:33 +00:00
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-13 11:38:33 +00:00
{
char pilot_signal [ 3 ] = " 1C " ;
2018-03-16 11:21:13 +00:00
galileo_e1_code_gen_sinboc11_float ( d_tracking_code , pilot_signal , d_acquisition_gnss_synchro - > PRN ) ;
galileo_e1_code_gen_sinboc11_float ( d_data_code , d_acquisition_gnss_synchro - > Signal , d_acquisition_gnss_synchro - > PRN ) ;
2018-03-13 11:38:33 +00:00
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
correlator_data_cpu . set_local_code_and_taps ( d_code_samples_per_chip * d_code_length_chips , d_data_code , d_prompt_data_shift ) ;
}
else
{
2018-03-16 11:21:13 +00:00
galileo_e1_code_gen_sinboc11_float ( d_tracking_code , d_acquisition_gnss_synchro - > Signal , d_acquisition_gnss_synchro - > PRN ) ;
2018-03-13 11:38:33 +00:00
}
}
2018-12-03 11:52:17 +00:00
else if ( systemName = = " Galileo " and signal_type = = " 5X " )
2018-03-13 11:38:33 +00:00
{
2018-12-03 15:25:11 +00:00
auto * aux_code = static_cast < gr_complex * > ( volk_gnsssdr_malloc ( sizeof ( gr_complex ) * d_code_length_chips , volk_gnsssdr_get_alignment ( ) ) ) ;
2018-03-13 11:38:33 +00:00
galileo_e5_a_code_gen_complex_primary ( aux_code , d_acquisition_gnss_synchro - > PRN , const_cast < char * > ( signal_type . c_str ( ) ) ) ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-13 11:38:33 +00:00
{
2019-02-22 09:47:24 +00:00
d_secondary_code_string = const_cast < std : : string * > ( & GALILEO_E5A_Q_SECONDARY_CODE [ d_acquisition_gnss_synchro - > PRN - 1 ] ) ;
2018-08-11 10:56:52 +00:00
for ( uint32_t i = 0 ; i < d_code_length_chips ; i + + )
2018-03-13 11:38:33 +00:00
{
d_tracking_code [ i ] = aux_code [ i ] . imag ( ) ;
2018-08-27 16:05:19 +00:00
d_data_code [ i ] = aux_code [ i ] . real ( ) ; //the same because it is generated the full signal (E5aI + E5aQ)
2018-03-13 11:38:33 +00:00
}
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
correlator_data_cpu . set_local_code_and_taps ( d_code_length_chips , d_data_code , d_prompt_data_shift ) ;
}
else
{
2018-08-11 10:56:52 +00:00
for ( uint32_t i = 0 ; i < d_code_length_chips ; i + + )
2018-03-13 11:38:33 +00:00
{
d_tracking_code [ i ] = aux_code [ i ] . real ( ) ;
}
}
2018-03-27 09:01:23 +00:00
volk_gnsssdr_free ( aux_code ) ;
2018-02-12 16:17:39 +00:00
}
2018-12-18 21:55:36 +00:00
else if ( systemName = = " Beidou " and signal_type = = " B1 " )
2018-07-10 21:30:15 +00:00
{
beidou_b1i_code_gen_float ( d_tracking_code , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2018-12-24 21:14:10 +00:00
// Update secondary code settings for geo satellites
2019-01-28 01:29:43 +00:00
if ( d_acquisition_gnss_synchro - > PRN > 0 and d_acquisition_gnss_synchro - > PRN < 6 )
2018-12-24 21:14:10 +00:00
{
2019-01-28 01:29:43 +00:00
d_symbols_per_bit = 2 ;
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
d_secondary = false ;
trk_parameters . track_pilot = false ;
interchange_iq = false ;
d_secondary_code_length = 0 ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_D2_SECONDARY_CODE_STR ) ;
2018-12-24 21:14:10 +00:00
2019-01-28 01:29:43 +00:00
// preamble bits to sampled symbols
d_preamble_length_symbols = 22 ;
d_preambles_symbols = static_cast < int32_t * > ( volk_gnsssdr_malloc ( 22 * sizeof ( int32_t ) , volk_gnsssdr_get_alignment ( ) ) ) ;
int32_t n = 0 ;
uint16_t preambles_bits [ BEIDOU_B1I_PREAMBLE_LENGTH_BITS ] = { 1 , 1 , 1 , 0 , 0 , 0 , 1 , 0 , 0 , 1 , 0 } ;
for ( uint16_t preambles_bit : preambles_bits )
{
2019-01-28 01:49:27 +00:00
for ( int32_t j = 0 ; j < d_symbols_per_bit ; j + + )
2019-01-28 01:29:43 +00:00
{
if ( preambles_bit = = 1 )
{
d_preambles_symbols [ n ] = 1 ;
}
else
{
d_preambles_symbols [ n ] = - 1 ;
}
n + + ;
}
}
2019-02-28 13:10:44 +00:00
d_symbol_history . set_capacity ( 22 ) ; // Change fixed buffer size
2019-01-28 01:29:43 +00:00
d_symbol_history . clear ( ) ;
}
2018-07-10 21:30:15 +00:00
}
2018-02-12 16:17:39 +00:00
2018-03-13 11:38:33 +00:00
multicorrelator_cpu . set_local_code_and_taps ( d_code_samples_per_chip * d_code_length_chips , d_tracking_code , d_local_code_shift_chips ) ;
2018-03-12 14:16:39 +00:00
std : : fill_n ( d_correlator_outs , d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2018-02-12 16:17:39 +00:00
d_carrier_lock_fail_counter = 0 ;
2018-02-13 14:20:30 +00:00
d_rem_code_phase_samples = 0.0 ;
2018-02-12 16:17:39 +00:00
d_rem_carr_phase_rad = 0.0 ;
d_rem_code_phase_chips = 0.0 ;
d_acc_carrier_phase_rad = 0.0 ;
2018-03-13 11:38:33 +00:00
d_cn0_estimation_counter = 0 ;
d_carrier_lock_test = 1.0 ;
d_CN0_SNV_dB_Hz = 0.0 ;
if ( d_veml )
{
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 3 ] = trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = trk_parameters . very_early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-13 11:38:33 +00:00
}
else
{
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 2 ] = trk_parameters . early_late_space_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-13 11:38:33 +00:00
}
2018-02-12 16:17:39 +00:00
2018-04-05 16:08:49 +00:00
d_code_loop_filter . set_DLL_BW ( trk_parameters . dll_bw_hz ) ;
d_carrier_loop_filter . set_PLL_BW ( trk_parameters . pll_bw_hz ) ;
2018-03-13 11:38:33 +00:00
d_carrier_loop_filter . set_pdi ( static_cast < float > ( d_code_period ) ) ;
d_code_loop_filter . set_pdi ( static_cast < float > ( d_code_period ) ) ;
2018-02-12 16:17:39 +00:00
// DEBUG OUTPUT
2018-04-12 18:45:13 +00:00
std : : cout < < " Tracking of " < < systemName < < " " < < signal_pretty_name < < " signal started on channel " < < d_channel < < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2018-11-29 16:09:30 +00:00
DLOG ( INFO ) < < " Starting tracking of satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < " on channel " < < d_channel ;
2018-02-12 16:17:39 +00:00
// enable tracking pull-in
d_state = 1 ;
2018-03-19 10:19:55 +00:00
d_cloop = true ;
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . clear ( ) ;
2018-03-13 11:38:33 +00:00
d_last_prompt = gr_complex ( 0.0 , 0.0 ) ;
2018-02-12 16:17:39 +00:00
}
dll_pll_veml_tracking : : ~ dll_pll_veml_tracking ( )
{
2018-12-03 11:52:17 +00:00
if ( signal_type = = " 1C " )
2018-07-13 13:23:20 +00:00
{
2018-12-24 21:14:10 +00:00
volk_gnsssdr_free ( d_preambles_symbols ) ;
2018-07-13 13:23:20 +00:00
}
2018-02-12 16:17:39 +00:00
if ( d_dump_file . is_open ( ) )
{
try
2018-03-12 14:16:39 +00:00
{
2018-02-12 16:17:39 +00:00
d_dump_file . close ( ) ;
2018-03-12 14:16:39 +00:00
}
catch ( const std : : exception & ex )
{
2018-02-12 16:17:39 +00:00
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
2018-10-29 23:47:12 +00:00
if ( d_dump_mat )
2018-02-12 16:17:39 +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-02-12 16:17:39 +00:00
}
try
2018-03-12 14:16:39 +00:00
{
2018-02-12 16:17:39 +00:00
volk_gnsssdr_free ( d_local_code_shift_chips ) ;
volk_gnsssdr_free ( d_correlator_outs ) ;
volk_gnsssdr_free ( d_tracking_code ) ;
2018-05-19 21:54:36 +00:00
volk_gnsssdr_free ( d_Prompt_Data ) ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-02-12 16:17:39 +00:00
{
volk_gnsssdr_free ( d_data_code ) ;
correlator_data_cpu . free ( ) ;
}
delete [ ] d_Prompt_buffer ;
multicorrelator_cpu . free ( ) ;
2018-03-12 14:16:39 +00:00
}
catch ( const std : : exception & ex )
{
2018-02-12 16:17:39 +00:00
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
bool dll_pll_veml_tracking : : acquire_secondary ( )
{
2018-03-29 07:23:23 +00:00
// ******* preamble correlation ********
2018-08-11 10:56:52 +00:00
int32_t corr_value = 0 ;
for ( uint32_t i = 0 ; i < d_secondary_code_length ; i + + )
2018-02-12 16:17:39 +00:00
{
2019-02-28 13:10:44 +00:00
if ( d_Prompt_circular_buffer [ i ] . real ( ) < 0.0 ) // symbols clipping
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
if ( d_secondary_code_string - > at ( i ) = = ' 0 ' )
2018-02-12 16:17:39 +00:00
{
corr_value + + ;
}
else
{
corr_value - - ;
}
}
else
{
2018-03-12 14:16:39 +00:00
if ( d_secondary_code_string - > at ( i ) = = ' 0 ' )
2018-02-12 16:17:39 +00:00
{
corr_value - - ;
}
else
{
corr_value + + ;
}
}
}
2018-08-12 22:54:23 +00:00
if ( abs ( corr_value ) = = static_cast < int32_t > ( d_secondary_code_length ) )
2018-02-12 16:17:39 +00:00
{
return true ;
}
2018-12-03 21:08:19 +00:00
2019-01-28 01:29:43 +00:00
return false ;
2018-02-12 16:17:39 +00:00
}
2018-04-09 19:09:25 +00:00
bool dll_pll_veml_tracking : : cn0_and_tracking_lock_status ( double coh_integration_time_s )
2018-02-12 16:17:39 +00:00
{
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
2018-04-28 10:33:46 +00:00
if ( d_cn0_estimation_counter < trk_parameters . cn0_samples )
2018-02-12 16:17:39 +00:00
{
// fill buffer with prompt correlator output values
d_Prompt_buffer [ d_cn0_estimation_counter ] = d_P_accu ;
d_cn0_estimation_counter + + ;
return true ;
}
2019-01-28 01:29:43 +00:00
d_cn0_estimation_counter = 0 ;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator ( d_Prompt_buffer , trk_parameters . cn0_samples , coh_integration_time_s ) ;
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector ( d_Prompt_buffer , trk_parameters . cn0_samples ) ;
// Loss of lock detection
if ( d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters . cn0_min )
{
d_carrier_lock_fail_counter + + ;
}
else
{
2019-02-11 20:13:02 +00:00
if ( d_carrier_lock_fail_counter > 0 )
{
d_carrier_lock_fail_counter - - ;
}
2019-01-28 01:29:43 +00:00
}
if ( d_carrier_lock_fail_counter > trk_parameters . max_lock_fail )
{
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! " < < std : : endl ;
LOG ( INFO ) < < " Loss of lock in channel " < < d_channel < < " ! " ;
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; // 3 -> loss of lock
d_carrier_lock_fail_counter = 0 ;
return false ;
}
return true ;
2018-02-12 16:17:39 +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
2018-03-12 14:16:39 +00:00
void dll_pll_veml_tracking : : do_correlation_step ( const gr_complex * input_samples )
2018-02-12 16:17:39 +00:00
{
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
2018-03-12 14:16:39 +00:00
multicorrelator_cpu . set_input_output_vectors ( d_correlator_outs , input_samples ) ;
2018-02-12 16:17:39 +00:00
multicorrelator_cpu . Carrier_wipeoff_multicorrelator_resampler (
2018-03-12 14:16:39 +00:00
d_rem_carr_phase_rad ,
2018-09-06 15:46:55 +00:00
d_carrier_phase_step_rad , d_carrier_phase_rate_step_rad ,
2018-03-16 11:21:13 +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 ) ,
2018-08-15 11:38:49 +00:00
static_cast < float > ( d_code_phase_rate_step_chips ) * static_cast < float > ( d_code_samples_per_chip ) ,
2018-04-05 16:08:49 +00:00
trk_parameters . vector_length ) ;
2018-02-12 16:17:39 +00:00
// DATA CORRELATOR (if tracking tracks the pilot signal)
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-02-12 16:17:39 +00:00
{
2018-03-12 14:16:39 +00:00
correlator_data_cpu . set_input_output_vectors ( d_Prompt_Data , input_samples ) ;
2018-02-12 16:17:39 +00:00
correlator_data_cpu . Carrier_wipeoff_multicorrelator_resampler (
2018-09-13 17:18:19 +00:00
d_rem_carr_phase_rad ,
2018-09-06 15:46:55 +00:00
d_carrier_phase_step_rad , d_carrier_phase_rate_step_rad ,
2018-03-16 11:21:13 +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 ) ,
2018-08-15 11:38:49 +00:00
static_cast < float > ( d_code_phase_rate_step_chips ) * static_cast < float > ( d_code_samples_per_chip ) ,
2018-04-05 16:08:49 +00:00
trk_parameters . vector_length ) ;
2018-02-12 16:17:39 +00:00
}
}
2018-03-19 10:19:55 +00:00
void dll_pll_veml_tracking : : run_dll_pll ( )
2018-02-12 16:17:39 +00:00
{
// ################## PLL ##########################################################
// PLL discriminator
2018-03-19 10:19:55 +00:00
if ( d_cloop )
2018-02-12 16:17:39 +00:00
{
2018-03-13 11:38:33 +00:00
// Costas loop discriminator, insensitive to 180 deg phase transitions
d_carr_error_hz = pll_cloop_two_quadrant_atan ( d_P_accu ) / PI_2 ;
2018-02-12 16:17:39 +00:00
}
else
{
2018-03-13 11:38:33 +00:00
// Secondary code acquired. No symbols transition should be present in the signal
d_carr_error_hz = pll_four_quadrant_atan ( d_P_accu ) / PI_2 ;
2018-02-12 16:17:39 +00:00
}
// Carrier discriminator filter
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_nco ( d_carr_error_hz ) ;
// New carrier Doppler frequency estimation
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz ;
2018-08-03 10:05:40 +00:00
2018-02-12 16:17:39 +00:00
// ################## DLL ##########################################################
// DLL discriminator
2018-03-13 11:38:33 +00:00
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
{
d_code_error_chips = dll_nc_e_minus_l_normalized ( d_E_accu , d_L_accu ) ; // [chips/Ti]
}
2018-02-12 16:17:39 +00:00
// Code discriminator filter
2018-03-12 14:16:39 +00:00
d_code_error_filt_chips = d_code_loop_filter . get_code_nco ( d_code_error_chips ) ; // [chips/second]
2018-08-03 10:05:40 +00:00
// New code Doppler frequency estimation
d_code_freq_chips = ( 1.0 + ( d_carrier_doppler_hz / d_signal_carrier_freq ) ) * d_code_chip_rate - d_code_error_filt_chips ;
2018-02-12 16:17:39 +00:00
}
void dll_pll_veml_tracking : : clear_tracking_vars ( )
{
2018-03-13 10:16:30 +00:00
std : : fill_n ( d_correlator_outs , d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2019-02-11 20:13:02 +00:00
if ( trk_parameters . track_pilot )
{
d_Prompt_Data [ 0 ] = gr_complex ( 0.0 , 0.0 ) ;
}
2018-02-12 16:17:39 +00:00
d_carr_error_hz = 0.0 ;
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-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . clear ( ) ;
2018-03-13 11:38:33 +00:00
d_last_prompt = gr_complex ( 0.0 , 0.0 ) ;
2018-09-13 17:18:19 +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-03-13 11:38:33 +00:00
}
2018-03-29 07:23:23 +00:00
2018-03-13 11:38:33 +00:00
void dll_pll_veml_tracking : : update_tracking_vars ( )
{
T_chip_seconds = 1.0 / d_code_freq_chips ;
T_prn_seconds = T_chip_seconds * static_cast < double > ( d_code_length_chips ) ;
2018-03-27 09:01:23 +00:00
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
2018-03-13 11:38:33 +00:00
// 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
2018-04-05 16:08:49 +00:00
T_prn_samples = T_prn_seconds * trk_parameters . fs_in ;
2018-08-03 10:05:40 +00:00
K_blk_samples = T_prn_samples + d_rem_code_phase_samples ;
2018-08-12 22:54:23 +00:00
//d_current_prn_length_samples = static_cast<int32_t>(round(K_blk_samples)); // round to a discrete number of samples
d_current_prn_length_samples = static_cast < int32_t > ( std : : floor ( K_blk_samples ) ) ; // round to a discrete number of samples
2018-03-13 11:38:33 +00:00
//################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
2018-04-05 16:08:49 +00:00
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters . fs_in ;
2018-09-08 21:20:29 +00:00
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
2018-09-09 11:40:31 +00:00
if ( trk_parameters . high_dyn )
2018-09-06 22:06:06 +00:00
{
2018-09-11 18:47:32 +00:00
d_carr_ph_history . push_back ( std : : pair < double , double > ( d_carrier_phase_step_rad , static_cast < double > ( d_current_prn_length_samples ) ) ) ;
2018-09-10 19:29:44 +00:00
if ( d_carr_ph_history . full ( ) )
2018-09-06 22:06:06 +00:00
{
2018-09-11 18:47:32 +00:00
double tmp_cp1 = 0.0 ;
double tmp_cp2 = 0.0 ;
double tmp_samples = 0.0 ;
for ( unsigned int k = 0 ; k < trk_parameters . smoother_length ; k + + )
{
2019-02-28 13:10:44 +00:00
tmp_cp1 + = d_carr_ph_history [ k ] . first ;
tmp_cp2 + = d_carr_ph_history [ trk_parameters . smoother_length * 2 - k - 1 ] . first ;
tmp_samples + = d_carr_ph_history [ trk_parameters . smoother_length * 2 - k - 1 ] . second ;
2018-09-11 18:47:32 +00:00
}
tmp_cp1 / = static_cast < double > ( trk_parameters . smoother_length ) ;
tmp_cp2 / = static_cast < double > ( trk_parameters . smoother_length ) ;
d_carrier_phase_rate_step_rad = ( tmp_cp2 - tmp_cp1 ) / tmp_samples ;
2018-09-06 22:06:06 +00:00
}
}
2018-09-18 22:59:13 +00:00
//std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl;
2018-03-13 11:38:33 +00:00
// remnant carrier phase to prevent overflow in the code NCO
2018-09-13 17:18:19 +00:00
d_rem_carr_phase_rad + = static_cast < float > ( d_carrier_phase_step_rad * static_cast < double > ( d_current_prn_length_samples ) + 0.5 * d_carrier_phase_rate_step_rad * static_cast < double > ( d_current_prn_length_samples ) * static_cast < double > ( d_current_prn_length_samples ) ) ;
d_rem_carr_phase_rad = fmod ( d_rem_carr_phase_rad , PI_2 ) ;
2018-09-09 11:40:31 +00:00
2018-03-13 11:38:33 +00:00
// carrier phase accumulator
2018-09-18 22:59:13 +00:00
//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);
//std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl;
2018-09-09 11:40:31 +00:00
d_acc_carrier_phase_rad - = ( d_carrier_phase_step_rad * static_cast < double > ( d_current_prn_length_samples ) + 0.5 * d_carrier_phase_rate_step_rad * static_cast < double > ( d_current_prn_length_samples ) * static_cast < double > ( d_current_prn_length_samples ) ) ;
2018-03-13 11:38:33 +00:00
//################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
2018-04-05 16:08:49 +00:00
d_code_phase_step_chips = d_code_freq_chips / trk_parameters . fs_in ;
2018-09-10 19:29:44 +00:00
if ( trk_parameters . high_dyn )
{
2018-09-11 18:47:32 +00:00
d_code_ph_history . push_back ( std : : pair < double , double > ( d_code_phase_step_chips , static_cast < double > ( d_current_prn_length_samples ) ) ) ;
2018-09-10 19:29:44 +00:00
if ( d_code_ph_history . full ( ) )
{
2018-09-11 18:47:32 +00:00
double tmp_cp1 = 0.0 ;
double tmp_cp2 = 0.0 ;
double tmp_samples = 0.0 ;
for ( unsigned int k = 0 ; k < trk_parameters . smoother_length ; k + + )
{
2019-02-28 13:10:44 +00:00
tmp_cp1 + = d_code_ph_history [ k ] . first ;
tmp_cp2 + = d_code_ph_history [ trk_parameters . smoother_length * 2 - k - 1 ] . first ;
tmp_samples + = d_code_ph_history [ trk_parameters . smoother_length * 2 - k - 1 ] . second ;
2018-09-11 18:47:32 +00:00
}
tmp_cp1 / = static_cast < double > ( trk_parameters . smoother_length ) ;
tmp_cp2 / = static_cast < double > ( trk_parameters . smoother_length ) ;
d_code_phase_rate_step_chips = ( tmp_cp2 - tmp_cp1 ) / tmp_samples ;
2018-09-10 19:29:44 +00:00
}
}
2018-03-13 11:38:33 +00:00
// remnant code phase [chips]
d_rem_code_phase_samples = K_blk_samples - static_cast < double > ( d_current_prn_length_samples ) ; // rounding error < 1 sample
2018-04-05 16:08:49 +00:00
d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters . fs_in ;
2018-03-13 11:38:33 +00:00
}
2018-03-29 07:23:23 +00:00
2018-03-13 11:38:33 +00:00
void dll_pll_veml_tracking : : save_correlation_results ( )
{
if ( d_secondary )
{
if ( d_secondary_code_string - > at ( d_current_symbol ) = = ' 0 ' )
{
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 ;
}
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 ;
}
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 ;
2018-03-19 10:19:55 +00:00
d_current_symbol + + ;
d_current_symbol % = d_symbols_per_bit ;
2018-03-13 11:38:33 +00:00
}
2018-03-19 10:19:55 +00:00
// If tracking pilot, disable Costas loop
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2019-02-11 20:13:02 +00:00
{
d_cloop = false ;
}
2018-03-19 10:19:55 +00:00
else
2019-02-11 20:13:02 +00:00
{
d_cloop = true ;
}
2018-02-12 16:17:39 +00:00
}
2018-03-19 10:19:55 +00:00
void dll_pll_veml_tracking : : log_data ( bool integrating )
2018-02-12 16:17:39 +00:00
{
2018-10-29 23:47:12 +00:00
if ( d_dump )
2018-02-12 16:17:39 +00:00
{
// Dump results to file
float prompt_I ;
float prompt_Q ;
float tmp_VE , tmp_E , tmp_P , tmp_L , tmp_VL ;
float tmp_float ;
double tmp_double ;
2018-08-10 18:34:03 +00:00
uint64_t tmp_long_int ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-13 11:38:33 +00:00
{
2018-03-16 11:21:13 +00:00
if ( interchange_iq )
{
prompt_I = d_Prompt_Data - > imag ( ) ;
prompt_Q = d_Prompt_Data - > real ( ) ;
}
else
{
prompt_I = d_Prompt_Data - > real ( ) ;
prompt_Q = d_Prompt_Data - > imag ( ) ;
}
2018-03-13 11:38:33 +00:00
}
else
{
2018-03-16 11:21:13 +00:00
if ( interchange_iq )
{
prompt_I = d_Prompt - > imag ( ) ;
prompt_Q = d_Prompt - > real ( ) ;
}
else
{
prompt_I = d_Prompt - > real ( ) ;
prompt_Q = d_Prompt - > imag ( ) ;
}
2018-03-13 11:38:33 +00:00
}
2018-03-12 14:16:39 +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 ;
}
2018-02-12 16:17:39 +00:00
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-19 10:19:55 +00:00
if ( integrating )
{
2018-03-19 15:52:36 +00:00
//TODO: Improve this solution!
2018-03-19 10:19:55 +00:00
// It compensates the amplitude difference while integrating
2018-03-29 13:19:21 +00:00
if ( d_extend_correlation_symbols_count > 0 )
{
2018-04-05 16:08:49 +00:00
float scale_factor = static_cast < float > ( trk_parameters . extend_correlation_symbols ) / static_cast < float > ( d_extend_correlation_symbols_count ) ;
2018-03-29 13:19:21 +00:00
tmp_VE * = scale_factor ;
tmp_E * = scale_factor ;
tmp_P * = scale_factor ;
tmp_L * = scale_factor ;
tmp_VL * = scale_factor ;
}
2018-03-19 10:19:55 +00:00
}
2018-02-12 16:17:39 +00:00
try
2018-03-12 14:16:39 +00:00
{
2018-02-12 16:17:39 +00:00
// Dump correlators output
2018-03-12 14:16:39 +00:00
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 ) ) ;
2018-02-12 16:17:39 +00:00
// PROMPT I and Q (to analyze navigation symbols)
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_I ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_Q ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// PRN start sample stamp
2018-08-12 22:54:23 +00:00
tmp_long_int = d_sample_counter + static_cast < uint64_t > ( d_current_prn_length_samples ) ;
2018-08-10 18:34:03 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_long_int ) , sizeof ( uint64_t ) ) ;
2018-02-12 16:17:39 +00:00
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_rad ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// carrier and code frequency
tmp_float = d_carrier_doppler_hz ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-09-09 11:40:31 +00:00
// carrier phase rate [Hz/s]
2018-09-08 21:20:29 +00:00
tmp_float = d_carrier_phase_rate_step_rad * trk_parameters . fs_in * trk_parameters . fs_in / PI_2 ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
tmp_float = d_code_freq_chips ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-09-10 19:29:44 +00:00
// code phase rate [chips/s^2]
tmp_float = d_code_phase_rate_step_chips * trk_parameters . fs_in * trk_parameters . fs_in ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// PLL commands
tmp_float = d_carr_error_hz ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
tmp_float = d_carr_error_filt_hz ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// DLL commands
tmp_float = d_code_error_chips ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
tmp_float = d_code_error_filt_chips ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// CN0 and carrier lock test
tmp_float = d_CN0_SNV_dB_Hz ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
tmp_float = d_carrier_lock_test ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-02-12 16:17:39 +00:00
tmp_double = static_cast < double > ( d_sample_counter + d_current_prn_length_samples ) ;
2018-03-12 14:16:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
2018-02-12 16:17:39 +00:00
// 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-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
catch ( const std : : ifstream : : failure & e )
2018-03-12 14:16:39 +00:00
{
2018-02-12 16:17:39 +00:00
LOG ( WARNING ) < < " Exception writing trk dump file " < < e . what ( ) ;
2018-03-12 14:16:39 +00:00
}
2018-02-12 16:17:39 +00:00
}
}
2018-08-11 10:56:52 +00:00
int32_t dll_pll_veml_tracking : : save_matfile ( )
2018-02-12 16:17:39 +00:00
{
2018-03-29 07:23:23 +00:00
// READ DUMP FILE
std : : ifstream : : pos_type size ;
2018-08-11 10:56:52 +00:00
int32_t number_of_double_vars = 1 ;
2018-09-10 19:29:44 +00:00
int32_t number_of_float_vars = 19 ;
2018-08-11 10:56:52 +00:00
int32_t epoch_size_bytes = sizeof ( uint64_t ) + sizeof ( double ) * number_of_double_vars +
sizeof ( float ) * number_of_float_vars + sizeof ( uint32_t ) ;
2018-03-29 07:23:23 +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 " ) ;
std : : cout < < " Generating .mat file for " < < dump_filename_ < < std : : endl ;
2018-03-29 07:23:23 +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-03-29 07:23:23 +00:00
}
catch ( const std : : ifstream : : failure & e )
{
std : : cerr < < " Problem opening dump file: " < < e . what ( ) < < std : : endl ;
return 1 ;
}
// count number of epochs and rewind
2018-08-10 18:34:03 +00:00
int64_t num_epoch = 0 ;
2018-03-29 07:23:23 +00:00
if ( dump_file . is_open ( ) )
{
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-03-29 07:23:23 +00:00
dump_file . seekg ( 0 , std : : ios : : beg ) ;
}
else
{
return 1 ;
}
2018-12-03 15:25:11 +00:00
auto * abs_VE = new float [ num_epoch ] ;
auto * abs_E = new float [ num_epoch ] ;
auto * abs_P = new float [ num_epoch ] ;
auto * abs_L = new float [ num_epoch ] ;
auto * abs_VL = new float [ num_epoch ] ;
auto * Prompt_I = new float [ num_epoch ] ;
auto * Prompt_Q = new float [ num_epoch ] ;
auto * PRN_start_sample_count = new uint64_t [ num_epoch ] ;
auto * acc_carrier_phase_rad = new float [ num_epoch ] ;
auto * carrier_doppler_hz = new float [ num_epoch ] ;
auto * carrier_doppler_rate_hz = new float [ num_epoch ] ;
auto * code_freq_chips = new float [ num_epoch ] ;
auto * code_freq_rate_chips = new float [ num_epoch ] ;
auto * carr_error_hz = new float [ num_epoch ] ;
auto * carr_error_filt_hz = new float [ num_epoch ] ;
auto * code_error_chips = new float [ num_epoch ] ;
auto * code_error_filt_chips = new float [ num_epoch ] ;
auto * CN0_SNV_dB_Hz = new float [ num_epoch ] ;
auto * carrier_lock_test = new float [ num_epoch ] ;
auto * aux1 = new float [ num_epoch ] ;
auto * aux2 = new double [ num_epoch ] ;
auto * PRN = new uint32_t [ num_epoch ] ;
2018-02-12 16:17:39 +00:00
2018-03-29 07:23:23 +00:00
try
2018-02-12 16:17:39 +00:00
{
2018-03-29 07:23:23 +00:00
if ( dump_file . is_open ( ) )
{
2018-08-10 18:34:03 +00:00
for ( int64_t i = 0 ; i < num_epoch ; i + + )
2018-03-29 07:23:23 +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-03-29 07:23:23 +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-09-08 21:20:29 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carrier_doppler_rate_hz [ i ] ) , sizeof ( float ) ) ;
2018-03-29 07:23:23 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_freq_chips [ i ] ) , sizeof ( float ) ) ;
2018-09-10 19:29:44 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_freq_rate_chips [ i ] ) , sizeof ( float ) ) ;
2018-03-29 07:23:23 +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-03-29 07:23:23 +00:00
}
}
dump_file . close ( ) ;
}
catch ( const std : : ifstream : : failure & e )
{
std : : cerr < < " Problem reading dump file: " < < e . what ( ) < < std : : endl ;
delete [ ] abs_VE ;
delete [ ] abs_E ;
delete [ ] abs_P ;
delete [ ] abs_L ;
delete [ ] abs_VL ;
delete [ ] Prompt_I ;
delete [ ] Prompt_Q ;
delete [ ] PRN_start_sample_count ;
delete [ ] acc_carrier_phase_rad ;
delete [ ] carrier_doppler_hz ;
2018-09-08 21:20:29 +00:00
delete [ ] carrier_doppler_rate_hz ;
2018-03-29 07:23:23 +00:00
delete [ ] code_freq_chips ;
2018-09-10 19:29:44 +00:00
delete [ ] code_freq_rate_chips ;
2018-03-29 07:23:23 +00:00
delete [ ] carr_error_hz ;
delete [ ] carr_error_filt_hz ;
delete [ ] code_error_chips ;
delete [ ] code_error_filt_chips ;
delete [ ] CN0_SNV_dB_Hz ;
delete [ ] carrier_lock_test ;
delete [ ] aux1 ;
delete [ ] aux2 ;
delete [ ] PRN ;
return 1 ;
}
2018-02-12 16:17:39 +00:00
2018-03-29 07:23:23 +00:00
// WRITE MAT FILE
mat_t * matfp ;
matvar_t * matvar ;
2018-10-29 23:47:12 +00:00
std : : string filename = dump_filename_ ;
2018-03-29 07:23:23 +00:00
filename . erase ( filename . length ( ) - 4 , 4 ) ;
filename . append ( " .mat " ) ;
2018-12-03 09:05:47 +00:00
matfp = Mat_CreateVer ( filename . c_str ( ) , nullptr , MAT_FT_MAT73 ) ;
if ( reinterpret_cast < int64_t * > ( matfp ) ! = nullptr )
2018-03-29 07:23:23 +00:00
{
size_t dims [ 2 ] = { 1 , static_cast < size_t > ( num_epoch ) } ;
2018-04-05 18:58:48 +00:00
matvar = Mat_VarCreate ( " abs_VE " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , abs_VE , 0 ) ;
2018-03-29 07:23:23 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-12 14:16:39 +00:00
2018-03-29 07:23:23 +00:00
matvar = Mat_VarCreate ( " abs_E " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , abs_E , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-16 11:21:13 +00:00
2018-03-29 07:23:23 +00:00
matvar = Mat_VarCreate ( " abs_P " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , abs_P , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-12 14:16:39 +00:00
2018-03-29 07:23:23 +00:00
matvar = Mat_VarCreate ( " abs_L " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , abs_L , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-04-05 18:58:48 +00:00
matvar = Mat_VarCreate ( " abs_VL " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , abs_VL , 0 ) ;
2018-03-29 07:23:23 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " Prompt_I " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , Prompt_I , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " Prompt_Q " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , Prompt_Q , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " PRN_start_sample_count " , MAT_C_UINT64 , MAT_T_UINT64 , 2 , dims , PRN_start_sample_count , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " acc_carrier_phase_rad " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , acc_carrier_phase_rad , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " carrier_doppler_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , carrier_doppler_hz , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-09-08 21:20:29 +00:00
matvar = Mat_VarCreate ( " carrier_doppler_rate_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , carrier_doppler_rate_hz , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-29 07:23:23 +00:00
matvar = Mat_VarCreate ( " code_freq_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , code_freq_chips , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-09-10 19:29:44 +00:00
matvar = Mat_VarCreate ( " code_freq_rate_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , code_freq_rate_chips , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-29 07:23:23 +00:00
matvar = Mat_VarCreate ( " carr_error_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , carr_error_hz , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " carr_error_filt_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , carr_error_filt_hz , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " code_error_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , code_error_chips , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " code_error_filt_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , code_error_filt_chips , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " CN0_SNV_dB_Hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , CN0_SNV_dB_Hz , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " carrier_lock_test " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , carrier_lock_test , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " aux1 " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims , aux1 , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " aux2 " , MAT_C_DOUBLE , MAT_T_DOUBLE , 2 , dims , aux2 , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
matvar = Mat_VarCreate ( " PRN " , MAT_C_UINT32 , MAT_T_UINT32 , 2 , dims , PRN , 0 ) ;
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
}
Mat_Close ( matfp ) ;
delete [ ] abs_VE ;
delete [ ] abs_E ;
delete [ ] abs_P ;
delete [ ] abs_L ;
delete [ ] abs_VL ;
delete [ ] Prompt_I ;
delete [ ] Prompt_Q ;
delete [ ] PRN_start_sample_count ;
delete [ ] acc_carrier_phase_rad ;
delete [ ] carrier_doppler_hz ;
2018-09-08 21:20:29 +00:00
delete [ ] carrier_doppler_rate_hz ;
2018-03-29 07:23:23 +00:00
delete [ ] code_freq_chips ;
2018-09-10 19:29:44 +00:00
delete [ ] code_freq_rate_chips ;
2018-03-29 07:23:23 +00:00
delete [ ] carr_error_hz ;
delete [ ] carr_error_filt_hz ;
delete [ ] code_error_chips ;
delete [ ] code_error_filt_chips ;
delete [ ] CN0_SNV_dB_Hz ;
delete [ ] carrier_lock_test ;
delete [ ] aux1 ;
delete [ ] aux2 ;
delete [ ] PRN ;
return 0 ;
}
2018-08-11 10:56:52 +00:00
void dll_pll_veml_tracking : : set_channel ( uint32_t channel )
2018-03-29 07:23:23 +00:00
{
gr : : thread : : scoped_lock l ( d_setlock ) ;
d_channel = channel ;
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-29 07:23:23 +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-03-29 07:23:23 +00:00
if ( ! d_dump_file . is_open ( ) )
{
try
{
2018-10-29 23:47:12 +00:00
//trk_parameters.dump_filename.append(boost::lexical_cast<std::string>(d_channel));
//trk_parameters.dump_filename.append(".dat");
2018-03-29 07:23:23 +00:00
d_dump_file . exceptions ( std : : ifstream : : failbit | std : : ifstream : : 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-03-29 07:23:23 +00:00
}
catch ( const std : : ifstream : : failure & e )
{
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening trk dump file " < < e . what ( ) ;
}
}
}
}
void dll_pll_veml_tracking : : set_gnss_synchro ( Gnss_Synchro * p_gnss_synchro )
{
gr : : thread : : scoped_lock l ( d_setlock ) ;
d_acquisition_gnss_synchro = p_gnss_synchro ;
}
2018-10-28 10:07:53 +00:00
void dll_pll_veml_tracking : : stop_tracking ( )
{
gr : : thread : : scoped_lock l ( d_setlock ) ;
d_state = 0 ;
}
2018-03-29 07:23:23 +00:00
int dll_pll_veml_tracking : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items ,
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
{
gr : : thread : : scoped_lock l ( d_setlock ) ;
2018-12-03 15:25:11 +00:00
const auto * in = reinterpret_cast < const gr_complex * > ( input_items [ 0 ] ) ;
auto * * out = reinterpret_cast < Gnss_Synchro * * > ( & output_items [ 0 ] ) ;
2018-03-29 07:23:23 +00:00
Gnss_Synchro current_synchro_data = Gnss_Synchro ( ) ;
switch ( d_state )
{
case 0 : // Standby - Consume samples at full throttle, do nothing
{
2018-08-12 22:54:23 +00:00
d_sample_counter + = static_cast < uint64_t > ( ninput_items [ 0 ] ) ;
2018-03-29 07:23:23 +00:00
consume_each ( ninput_items [ 0 ] ) ;
return 0 ;
2018-03-12 14:16:39 +00:00
break ;
}
2018-03-29 07:23:23 +00:00
case 1 : // Pull-in
2018-03-12 14:16:39 +00:00
{
2018-03-29 07:23:23 +00:00
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
2018-11-29 16:09:30 +00:00
int64_t acq_trk_diff_samples = static_cast < int64_t > ( d_sample_counter ) - static_cast < int64_t > ( d_acq_sample_stamp ) ;
double acq_trk_diff_seconds = static_cast < double > ( acq_trk_diff_samples ) / trk_parameters . fs_in ;
double delta_trk_to_acq_prn_start_samples = static_cast < double > ( acq_trk_diff_samples ) - d_acq_code_phase_samples ;
// Doppler effect Fd = (C / (C + Vr)) * F
double radial_velocity = ( d_signal_carrier_freq + d_acq_carrier_doppler_hz ) / d_signal_carrier_freq ;
// new chip and PRN sequence periods based on acq Doppler
d_code_freq_chips = radial_velocity * d_code_chip_rate ;
d_code_freq_chips = d_code_chip_rate ;
d_code_phase_step_chips = d_code_freq_chips / trk_parameters . fs_in ;
d_code_phase_rate_step_chips = 0.0 ;
double T_chip_mod_seconds = 1.0 / d_code_freq_chips ;
double T_prn_mod_seconds = T_chip_mod_seconds * static_cast < double > ( d_code_length_chips ) ;
double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters . fs_in ;
d_acq_code_phase_samples = T_prn_mod_samples - std : : fmod ( delta_trk_to_acq_prn_start_samples , T_prn_mod_samples ) ;
d_current_prn_length_samples = round ( T_prn_mod_samples ) ;
int32_t samples_offset = round ( d_acq_code_phase_samples ) ;
d_acc_carrier_phase_rad - = d_carrier_phase_step_rad * static_cast < double > ( samples_offset ) ;
2018-03-29 07:23:23 +00:00
d_state = 2 ;
2018-11-29 16:09:30 +00:00
d_sample_counter + = samples_offset ; // count for the processed samples
DLOG ( 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-01-28 01:29:43 +00:00
consume_each ( samples_offset ) ; // shift input to perform alignment with local replica
2018-03-29 07:23:23 +00:00
return 0 ;
2018-03-12 14:16:39 +00:00
}
2018-03-29 07:23:23 +00:00
case 2 : // Wide tracking and symbol synchronization
2018-03-12 14:16:39 +00:00
{
do_correlation_step ( in ) ;
2018-03-29 07:23:23 +00:00
// 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 ;
2018-03-12 14:16:39 +00:00
2018-03-29 07:23:23 +00:00
// Check lock status
2018-04-28 10:33:46 +00:00
if ( ! cn0_and_tracking_lock_status ( d_code_period ) )
2018-03-12 14:16:39 +00:00
{
clear_tracking_vars ( ) ;
d_state = 0 ; // loss-of-lock detected
}
else
{
2018-03-29 07:23:23 +00:00
bool next_state = false ;
// Perform DLL/PLL tracking loop computations. Costas Loop enabled
2018-03-19 10:19:55 +00:00
run_dll_pll ( ) ;
2018-03-13 11:38:33 +00:00
update_tracking_vars ( ) ;
2018-03-12 14:16:39 +00:00
2018-03-29 07:23:23 +00:00
// enable write dump file this cycle (valid DLL/PLL cycle)
log_data ( false ) ;
if ( d_secondary )
2018-03-13 11:38:33 +00:00
{
2018-03-29 07:23:23 +00:00
// ####### SECONDARY CODE LOCK #####
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . push_back ( * d_Prompt ) ;
if ( d_Prompt_circular_buffer . size ( ) = = d_secondary_code_length )
2018-03-16 11:21:13 +00:00
{
2018-03-29 07:23:23 +00:00
next_state = acquire_secondary ( ) ;
if ( next_state )
{
2019-01-28 01:29:43 +00:00
LOG ( INFO ) < < systemName < < " " < < signal_pretty_name < < " secondary code locked in channel " < < d_channel
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2018-04-12 18:45:13 +00:00
std : : cout < < systemName < < " " < < signal_pretty_name < < " secondary code locked in channel " < < d_channel
2018-04-10 23:22:11 +00:00
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2018-03-29 07:23:23 +00:00
}
}
}
2018-04-03 09:01:59 +00:00
else if ( d_symbols_per_bit > 1 ) //Signal does not have secondary code. Search a bit transition by sign change
2018-03-29 07:23:23 +00:00
{
2018-07-13 13:23:20 +00:00
float current_tracking_time_s = static_cast < float > ( d_sample_counter - d_acq_sample_stamp ) / trk_parameters . fs_in ;
if ( current_tracking_time_s > 10 )
2018-03-29 07:23:23 +00:00
{
2018-07-13 13:23:20 +00:00
d_symbol_history . push_back ( d_Prompt - > real ( ) ) ;
//******* preamble correlation ********
2018-08-11 10:56:52 +00:00
int32_t corr_value = 0 ;
2019-01-28 01:54:02 +00:00
if ( ( static_cast < int32_t > ( d_symbol_history . size ( ) ) = = d_preamble_length_symbols ) ) // and (d_make_correlation or !d_flag_frame_sync))
2018-03-29 07:23:23 +00:00
{
2019-02-28 13:10:44 +00:00
int i = 0 ;
for ( const auto & iter : d_symbol_history )
2018-07-13 13:23:20 +00:00
{
2019-02-28 13:10:44 +00:00
if ( iter < 0.0 ) // symbols clipping
2018-07-13 13:23:20 +00:00
{
2018-12-24 21:14:10 +00:00
corr_value - = d_preambles_symbols [ i ] ;
2018-07-13 13:23:20 +00:00
}
else
{
2018-12-24 21:14:10 +00:00
corr_value + = d_preambles_symbols [ i ] ;
2018-07-13 13:23:20 +00:00
}
2019-02-28 13:10:44 +00:00
i + + ;
2018-07-13 13:23:20 +00:00
}
2018-03-29 07:23:23 +00:00
}
2018-12-24 21:14:10 +00:00
if ( corr_value = = d_preamble_length_symbols )
2018-03-29 07:23:23 +00:00
{
2019-01-28 01:29:43 +00:00
LOG ( INFO ) < < systemName < < " " < < signal_pretty_name < < " tracking preamble detected in channel " < < d_channel
2018-12-26 18:54:23 +00:00
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2018-07-13 13:23:20 +00:00
next_state = true ;
2018-03-29 07:23:23 +00:00
}
else
{
2018-07-13 13:23:20 +00:00
next_state = false ;
2018-03-29 07:23:23 +00:00
}
}
2018-07-13 13:23:20 +00:00
else
2018-03-29 07:23:23 +00:00
{
2018-07-13 13:23:20 +00:00
next_state = false ;
}
}
else
{
next_state = true ;
}
// ########### Output the tracking results to Telemetry block ##########
if ( interchange_iq )
{
if ( trk_parameters . track_pilot )
{
// Note that data and pilot components are in quadrature. I and Q are interchanged
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
2018-03-29 07:23:23 +00:00
}
else
{
2018-07-13 13:23:20 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
2018-03-29 07:23:23 +00:00
}
}
2018-04-03 09:01:59 +00:00
else
{
2018-07-13 13:23:20 +00:00
if ( trk_parameters . track_pilot )
{
// Note that data and pilot components are in quadrature. I and Q are interchanged
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
}
2018-04-03 09:01:59 +00:00
}
2018-12-24 21:14:10 +00:00
2018-07-13 13:23:20 +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 . Flag_valid_symbol_output = true ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
2018-03-29 07:23:23 +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 ) ;
2018-03-13 11:38:33 +00:00
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 ) ;
d_last_prompt = gr_complex ( 0.0 , 0.0 ) ;
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . clear ( ) ;
2018-03-13 11:38:33 +00:00
d_current_symbol = 0 ;
2018-03-12 14:16:39 +00:00
2018-03-13 11:38:33 +00:00
if ( d_enable_extended_integration )
{
2018-03-20 09:33:02 +00:00
// UPDATE INTEGRATION TIME
2018-03-13 11:38:33 +00:00
d_extend_correlation_symbols_count = 0 ;
2018-04-05 16:08:49 +00:00
float new_correlation_time = static_cast < float > ( trk_parameters . extend_correlation_symbols ) * static_cast < float > ( d_code_period ) ;
2018-03-19 15:52:36 +00:00
d_carrier_loop_filter . set_pdi ( new_correlation_time ) ;
d_code_loop_filter . set_pdi ( new_correlation_time ) ;
2018-03-13 11:38:33 +00:00
d_state = 3 ; // next state is the extended correlator integrator
2018-08-12 22:54:23 +00:00
LOG ( INFO ) < < " Enabled " < < trk_parameters . extend_correlation_symbols * static_cast < int32_t > ( d_code_period * 1000.0 ) < < " ms extended correlator in channel "
2018-03-13 11:38:33 +00:00
< < d_channel
2018-04-10 17:54:59 +00:00
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) ;
2018-08-12 22:54:23 +00:00
std : : cout < < " Enabled " < < trk_parameters . extend_correlation_symbols * static_cast < int32_t > ( d_code_period * 1000.0 ) < < " ms extended correlator in channel "
2018-03-13 11:38:33 +00:00
< < d_channel
2018-04-10 17:54:59 +00:00
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
2018-03-20 09:33:02 +00:00
// Set narrow taps delay values [chips]
2018-04-05 16:08:49 +00:00
d_code_loop_filter . set_DLL_BW ( trk_parameters . dll_bw_narrow_hz ) ;
d_carrier_loop_filter . set_PLL_BW ( trk_parameters . pll_bw_narrow_hz ) ;
2018-03-20 09:33:02 +00:00
if ( d_veml )
{
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . very_early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 1 ] = - trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 3 ] = trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 4 ] = trk_parameters . very_early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-20 09:33:02 +00:00
}
else
{
2018-04-05 16:08:49 +00:00
d_local_code_shift_chips [ 0 ] = - trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
d_local_code_shift_chips [ 2 ] = trk_parameters . early_late_space_narrow_chips * static_cast < float > ( d_code_samples_per_chip ) ;
2018-03-20 09:33:02 +00:00
}
2018-03-13 11:38:33 +00:00
}
else
{
d_state = 4 ;
2018-03-12 14:16:39 +00:00
}
}
}
break ;
}
case 3 : // coherent integration (correlation time extension)
{
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
// perform a correlation step
do_correlation_step ( in ) ;
2018-03-13 11:38:33 +00:00
update_tracking_vars ( ) ;
save_correlation_results ( ) ;
// ########### Output the tracking results to Telemetry block ##########
if ( interchange_iq )
2018-03-12 14:16:39 +00:00
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-16 11:21:13 +00:00
{
2018-03-29 07:23:23 +00:00
// Note that data and pilot components are in quadrature. I and Q are interchanged
2018-03-16 11:21:13 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
}
2018-03-12 14:16:39 +00:00
}
else
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-16 11:21:13 +00:00
{
2018-03-29 07:23:23 +00:00
// Note that data and pilot components are in quadrature. I and Q are interchanged
2018-03-16 11:21:13 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
}
2018-03-12 14:16:39 +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 . Flag_valid_symbol_output = true ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
d_extend_correlation_symbols_count + + ;
2018-04-05 16:08:49 +00:00
if ( d_extend_correlation_symbols_count = = ( trk_parameters . extend_correlation_symbols - 1 ) )
2018-03-12 14:16:39 +00:00
{
d_extend_correlation_symbols_count = 0 ;
d_state = 4 ;
}
2018-03-19 10:19:55 +00:00
log_data ( true ) ;
2018-03-12 14:16:39 +00:00
break ;
}
case 4 : // narrow tracking
{
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
2018-03-13 11:38:33 +00:00
2018-03-12 14:16:39 +00:00
// perform a correlation step
do_correlation_step ( in ) ;
2018-03-13 11:38:33 +00:00
save_correlation_results ( ) ;
2018-03-12 14:16:39 +00:00
// check lock status
2018-04-28 10:33:46 +00:00
if ( ! cn0_and_tracking_lock_status ( d_code_period * static_cast < double > ( trk_parameters . extend_correlation_symbols ) ) )
2018-03-12 14:16:39 +00:00
{
clear_tracking_vars ( ) ;
d_state = 0 ; // loss-of-lock detected
}
else
{
2018-03-19 10:19:55 +00:00
run_dll_pll ( ) ;
2018-03-13 11:38:33 +00:00
update_tracking_vars ( ) ;
2018-03-12 14:16:39 +00:00
// ########### Output the tracking results to Telemetry block ##########
2018-03-13 11:38:33 +00:00
if ( interchange_iq )
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-16 11:21:13 +00:00
{
2018-03-29 07:23:23 +00:00
// Note that data and pilot components are in quadrature. I and Q are interchanged
2018-03-16 11:21:13 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
}
2018-03-13 11:38:33 +00:00
}
else
{
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-16 11:21:13 +00:00
{
2018-03-29 07:23:23 +00:00
// Note that data and pilot components are in quadrature. I and Q are interchanged
2018-03-16 11:21:13 +00:00
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt_Data ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt_Data ) . imag ( ) ) ;
}
else
{
current_synchro_data . Prompt_I = static_cast < double > ( ( * d_Prompt ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( * d_Prompt ) . imag ( ) ) ;
}
2018-03-13 11:38:33 +00:00
}
2018-03-12 14:16:39 +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 . Flag_valid_symbol_output = true ;
current_synchro_data . correlation_length_ms = d_correlation_length_ms ;
// enable write dump file this cycle (valid DLL/PLL cycle)
2018-03-19 10:19:55 +00:00
log_data ( false ) ;
2018-03-12 14:16:39 +00:00
// 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 ) ;
2018-03-13 11:38:33 +00:00
if ( d_enable_extended_integration )
{
2018-03-29 07:23:23 +00:00
d_state = 3 ; // new coherent integration (correlation time extension) cycle
2018-03-13 11:38:33 +00:00
}
2018-03-12 14:16:39 +00:00
}
}
2018-02-12 16:17:39 +00:00
}
2018-03-12 14:16:39 +00:00
consume_each ( d_current_prn_length_samples ) ;
2018-08-11 10:56:52 +00:00
d_sample_counter + = static_cast < uint64_t > ( d_current_prn_length_samples ) ;
2018-02-12 16:17:39 +00:00
if ( current_synchro_data . Flag_valid_symbol_output )
{
2018-08-10 18:34:03 +00:00
current_synchro_data . fs = static_cast < int64_t > ( trk_parameters . fs_in ) ;
2018-03-12 14:16:39 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter ;
* out [ 0 ] = current_synchro_data ;
2018-02-12 16:17:39 +00:00
return 1 ;
}
2018-03-13 10:16:30 +00:00
return 0 ;
2018-02-12 16:17:39 +00:00
}