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"
2019-02-05 23:25:16 +00:00
# include "Beidou_B3I.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"
2019-03-18 07:43:38 +00:00
# include "beidou_b3i_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"
2019-03-05 07:59:04 +00:00
# include "gnss_synchro.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-03-13 11:38:33 +00:00
# include <glog/logging.h>
2019-03-05 07:59:04 +00:00
# include <gnuradio/io_signature.h> // for io_signature
# include <gnuradio/thread/thread.h> // for scoped_lock
# include <matio.h> // for Mat_VarCreate
# include <pmt/pmt_sugar.h> // for mp
2018-03-13 11:38:33 +00:00
# include <volk_gnsssdr/volk_gnsssdr.h>
2019-03-05 07:59:04 +00:00
# include <algorithm> // for fill_n
2019-07-14 12:09:12 +00:00
# include <array>
2019-03-05 07:59:04 +00:00
# include <cmath> // for fmod, round, floor
# include <exception> // for exception
2019-07-01 09:44:04 +00:00
# include <gsl/gsl>
# include <iostream> // for cout, cerr
2019-03-18 15:28:49 +00:00
# include <map>
2019-06-18 12:41:50 +00:00
# include <numeric>
2018-02-12 16:17:39 +00:00
2019-06-10 19:41:13 +00:00
# if HAS_STD_FILESYSTEM
2019-06-12 18:39:29 +00:00
# if HAS_STD_FILESYSTEM_EXPERIMENTAL
# include <experimental/filesystem>
namespace fs = std : : experimental : : filesystem ;
# else
2019-06-10 19:41:13 +00:00
# include <filesystem>
namespace fs = std : : filesystem ;
2019-06-12 18:39:29 +00:00
# endif
2019-06-10 19:41:13 +00:00
# else
# include <boost/filesystem/path.hpp>
namespace fs = boost : : filesystem ;
# endif
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-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
{
2019-07-04 15:19:33 +00:00
//prevent telemetry symbols accumulation in output buffers
2019-07-04 10:54:45 +00:00
this - > set_max_noutput_items ( 1 ) ;
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
2019-03-18 15:28:49 +00:00
// Telemetry message port input
this - > message_port_register_in ( pmt : : mp ( " telemetry_to_trk " ) ) ;
this - > set_msg_handler ( pmt : : mp ( " telemetry_to_trk " ) , boost : : bind ( & dll_pll_veml_tracking : : msg_handler_telemetry_to_trk , this , _1 ) ) ;
2018-07-13 13:23:20 +00:00
2018-02-12 16:17:39 +00:00
// initialize internal vars
2019-07-03 16:57:26 +00:00
d_dll_filt_history . set_capacity ( 1000 ) ;
2018-03-13 11:38:33 +00:00
d_veml = false ;
2018-03-19 10:19:55 +00:00
d_cloop = true ;
2019-03-18 15:28:49 +00:00
d_pull_in_transitory = 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 ;
2019-07-01 09:00:38 +00:00
d_data_secondary_code_length = 0U ;
d_data_secondary_code_string = 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 " ;
2019-01-25 21:43:00 +00:00
map_signal_pretty_name [ " B3 " ] = " B3I " ;
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 ;
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 ;
2019-07-01 09:00:38 +00:00
// symbol integration: 20 trk symbols (20 ms) = 1 tlm bit
// set the preamble in the secondary code acquisition to obtain tlm symbol synchronization
d_secondary_code_length = static_cast < uint32_t > ( GPS_CA_PREAMBLE_LENGTH_SYMBOLS ) ;
d_secondary_code_string = const_cast < std : : string * > ( & GPS_CA_PREAMBLE_SYMBOLS_STR ) ;
d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT ;
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 ) ;
2019-07-01 09:44:04 +00:00
// GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required
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-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 ;
2019-07-01 09:00:38 +00:00
// symbol integration: 10 trk symbols (10 ms) = 1 tlm bit
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-07-01 09:44:04 +00:00
// synchronize pilot secondary code
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 ) ;
2019-07-01 09:44:04 +00:00
// remove data secondary code
// remove Neuman-Hofman Code (see IS-GPS-705D)
2019-07-01 09:00:38 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( GPS_L5I_NH_CODE_LENGTH ) ;
d_data_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 + " Q " ;
2018-03-13 11:38:33 +00:00
}
else
{
2019-07-01 09:44:04 +00:00
// synchronize and remove data secondary code
// remove Neuman-Hofman Code (see IS-GPS-705D)
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-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 ;
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 ) ;
2019-07-01 09:44:04 +00:00
// Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required
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
}
2019-07-01 09:00:38 +00:00
// 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 ) ;
2019-07-01 09:00:38 +00:00
d_secondary = true ;
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2018-03-12 14:16:39 +00:00
{
2019-07-01 09:44:04 +00:00
// synchronize pilot secondary code
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 " ;
2019-07-01 09:44:04 +00:00
// remove data secondary code
2019-07-01 09:00:38 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_I_SECONDARY_CODE_LENGTH ) ;
d_data_secondary_code_string = const_cast < std : : string * > ( & GALILEO_E5A_I_SECONDARY_CODE ) ;
2018-03-12 14:16:39 +00:00
}
else
{
2019-07-01 09:44:04 +00:00
// synchronize and remove data secondary code
2019-07-01 09:00:38 +00:00
d_secondary_code_length = static_cast < uint32_t > ( GALILEO_E5A_I_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & GALILEO_E5A_I_SECONDARY_CODE ) ;
2018-04-28 10:33:46 +00:00
signal_pretty_name = signal_pretty_name + " I " ;
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 ;
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 ;
2019-03-01 21:16:37 +00:00
d_code_length_chips = static_cast < uint32_t > ( BEIDOU_B1I_CODE_LENGTH_CHIPS ) ;
2019-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-01-28 01:29:43 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
2019-07-10 17:52:40 +00:00
d_secondary = true ;
2019-01-28 01:29:43 +00:00
trk_parameters . track_pilot = false ;
2019-07-01 09:44:04 +00:00
// synchronize and remove data secondary code
2019-03-01 21:16:37 +00:00
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B1I_SECONDARY_CODE_LENGTH ) ;
2019-01-28 01:29:43 +00:00
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_SECONDARY_CODE_STR ) ;
2019-07-10 17:52:40 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B1I_SECONDARY_CODE_LENGTH ) ;
d_data_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_SECONDARY_CODE_STR ) ;
2018-07-10 21:30:15 +00:00
}
2019-01-25 21:43:00 +00:00
else if ( signal_type = = " B3 " )
2019-03-18 07:43:38 +00:00
{
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B3I_FREQ_HZ ;
d_code_period = BEIDOU_B3I_CODE_PERIOD ;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ ;
d_code_length_chips = static_cast < uint32_t > ( BEIDOU_B3I_CODE_LENGTH_CHIPS ) ;
2019-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-03-18 07:43:38 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
2019-07-05 18:30:34 +00:00
d_secondary = false ;
2019-03-18 07:43:38 +00:00
trk_parameters . track_pilot = false ;
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B3I_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B3I_SECONDARY_CODE_STR ) ;
2019-07-10 17:52:40 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B3I_SECONDARY_CODE_LENGTH ) ;
d_data_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B3I_SECONDARY_CODE_STR ) ;
2019-03-01 19:28:21 +00:00
}
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 ;
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 ;
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 ==========================================
2019-03-18 15:28:49 +00:00
d_code_loop_filter = Tracking_loop_filter ( d_code_period , trk_parameters . dll_bw_hz , trk_parameters . dll_filter_order , false ) ;
d_carrier_loop_filter . set_params ( trk_parameters . fll_bw_hz , trk_parameters . pll_bw_hz , trk_parameters . pll_filter_order ) ;
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 ) ;
2019-03-19 06:53:21 +00:00
d_current_correlation_time_s = 0.0 ;
2018-02-12 16:17:39 +00:00
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0 ;
2019-06-30 13:15:27 +00:00
d_Prompt_buffer = std : : vector < 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 ;
2019-06-11 18:20:23 +00:00
d_code_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 ( ) ) ) ;
2019-04-25 06:50:21 +00:00
d_cn0_smoother = Exponential_Smoother ( ) ;
2019-04-25 13:30:11 +00:00
if ( d_code_period > 0.0 )
{
d_cn0_smoother . set_samples_for_initialization ( 200 / static_cast < int > ( d_code_period * 1000.0 ) ) ;
}
2019-06-11 18:20:23 +00:00
d_carrier_lock_test_smoother = Exponential_Smoother ( ) ;
d_carrier_lock_test_smoother . set_alpha ( 0.002 ) ;
d_carrier_lock_test_smoother . set_min_value ( - 1.0 ) ;
d_carrier_lock_test_smoother . set_offset ( 0.0 ) ;
d_carrier_lock_test_smoother . set_samples_for_initialization ( 25 ) ;
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-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
}
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 ) )
{
std : : cerr < < " GNSS-SDR cannot create dump files for the tracking block. Wrong permissions? " < < std : : endl ;
d_dump = false ;
}
}
2019-06-14 10:52:46 +00:00
d_corrected_doppler = false ;
2018-02-12 16:17:39 +00:00
}
2019-03-19 06:53:21 +00:00
void dll_pll_veml_tracking : : forecast ( int noutput_items ,
gr_vector_int & ninput_items_required )
{
if ( noutput_items ! = 0 )
{
ninput_items_required [ 0 ] = static_cast < int32_t > ( trk_parameters . vector_length ) * 2 ;
}
}
void dll_pll_veml_tracking : : msg_handler_telemetry_to_trk ( const pmt : : pmt_t & msg )
{
try
{
if ( pmt : : any_ref ( msg ) . type ( ) = = typeid ( int ) )
{
int tlm_event ;
tlm_event = boost : : any_cast < int > ( pmt : : any_ref ( msg ) ) ;
2019-07-19 16:23:36 +00:00
if ( tlm_event = = 1 )
2019-03-19 06:53:21 +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 ) ;
d_carrier_lock_fail_counter = 200000 ; //force loss-of-lock condition
2019-03-19 06:53:21 +00:00
}
}
}
catch ( boost : : bad_any_cast & e )
{
LOG ( WARNING ) < < " msg_handler_telemetry_to_trk Bad any cast! " ;
}
}
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 ) ;
2019-07-01 09:44:04 +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 ( ) ;
2019-07-14 21:34:07 +00:00
std : : array < char , 3 > Signal_ { } ;
Signal_ [ 0 ] = d_acquisition_gnss_synchro - > Signal [ 0 ] ;
Signal_ [ 1 ] = d_acquisition_gnss_synchro - > Signal [ 1 ] ;
Signal_ [ 2 ] = d_acquisition_gnss_synchro - > Signal [ 2 ] ;
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
{
2019-07-01 09:44:04 +00:00
gps_l1_ca_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , 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
{
2019-07-01 09:44:04 +00:00
gps_l2c_m_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , d_acquisition_gnss_synchro - > PRN ) ;
2018-03-13 11:38:33 +00:00
}
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
{
2019-07-01 09:44:04 +00:00
gps_l5q_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , d_acquisition_gnss_synchro - > PRN ) ;
gps_l5i_code_gen_float ( gsl : : span < float > ( d_data_code , 2 * d_code_length_chips ) , 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_length_chips , d_data_code , d_prompt_data_shift ) ;
}
else
{
2019-07-01 09:44:04 +00:00
gps_l5i_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , 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 = = " 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
{
2019-07-01 09:44:04 +00:00
std : : array < char , 3 > pilot_signal = { { ' 1 ' , ' C ' , ' \0 ' } } ;
galileo_e1_code_gen_sinboc11_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , pilot_signal , d_acquisition_gnss_synchro - > PRN ) ;
galileo_e1_code_gen_sinboc11_float ( gsl : : span < float > ( d_data_code , 2 * d_code_length_chips ) , 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
{
2019-07-01 09:44:04 +00:00
galileo_e1_code_gen_sinboc11_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , 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 ( ) ) ) ;
2019-07-01 09:44:04 +00:00
std : : array < char , 3 > signal_type_ = { { ' 5 ' , ' X ' , ' \0 ' } } ;
galileo_e5_a_code_gen_complex_primary ( gsl : : span < gr_complex > ( aux_code , d_code_length_chips ) , d_acquisition_gnss_synchro - > PRN , signal_type_ ) ;
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 ( ) ;
2019-07-01 09:44:04 +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
{
2019-07-01 09:44:04 +00:00
beidou_b1i_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2019-07-03 16:57:26 +00:00
// GEO Satellites use different secondary code
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-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-01-28 01:29:43 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
d_secondary = false ;
trk_parameters . track_pilot = false ;
2019-07-01 09:00:38 +00:00
// set the preamble in the secondary code acquisition
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR ) ;
2019-07-03 16:57:26 +00:00
d_data_secondary_code_length = 0 ;
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
}
else
{
2019-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-07-03 16:57:26 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
2019-07-15 01:09:52 +00:00
d_secondary = true ;
2019-07-03 16:57:26 +00:00
trk_parameters . track_pilot = false ;
// synchronize and remove data secondary code
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B1I_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_SECONDARY_CODE_STR ) ;
2019-07-10 17:52:40 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B1I_SECONDARY_CODE_LENGTH ) ;
d_data_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B1I_SECONDARY_CODE_STR ) ;
2019-07-01 09:00:38 +00:00
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
2018-12-24 21:14:10 +00:00
}
2018-07-10 21:30:15 +00:00
}
2018-02-12 16:17:39 +00:00
2019-01-25 21:43:00 +00:00
else if ( systemName = = " Beidou " and signal_type = = " B3 " )
{
2019-07-01 09:44:04 +00:00
beidou_b3i_code_gen_float ( gsl : : span < float > ( d_tracking_code , 2 * d_code_length_chips ) , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2019-01-25 21:43:00 +00:00
// Update secondary code settings for geo satellites
2019-03-18 07:43:38 +00:00
if ( d_acquisition_gnss_synchro - > PRN > 0 and d_acquisition_gnss_synchro - > PRN < 6 )
2019-01-25 21:43:00 +00:00
{
2019-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-03-18 07:43:38 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
2019-07-15 01:09:52 +00:00
d_secondary = false ;
2019-03-18 07:43:38 +00:00
trk_parameters . track_pilot = false ;
2019-07-01 09:00:38 +00:00
// set the preamble in the secondary code acquisition
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR ) ;
2019-07-03 16:57:26 +00:00
d_data_secondary_code_length = 0 ;
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
}
else
{
2019-07-10 17:52:40 +00:00
d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT ; //todo: enable after fixing beidou symbol synchronization
2019-07-03 16:57:26 +00:00
d_correlation_length_ms = 1 ;
d_code_samples_per_chip = 1 ;
2019-07-10 17:52:40 +00:00
d_secondary = true ;
2019-07-03 16:57:26 +00:00
trk_parameters . track_pilot = false ;
// synchronize and remove data secondary code
d_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B3I_SECONDARY_CODE_LENGTH ) ;
d_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B3I_SECONDARY_CODE_STR ) ;
2019-07-10 17:52:40 +00:00
d_data_secondary_code_length = static_cast < uint32_t > ( BEIDOU_B3I_SECONDARY_CODE_LENGTH ) ;
d_data_secondary_code_string = const_cast < std : : string * > ( & BEIDOU_B3I_SECONDARY_CODE_STR ) ;
2019-07-01 09:00:38 +00:00
d_Prompt_circular_buffer . set_capacity ( d_secondary_code_length ) ;
2019-01-25 21:43:00 +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 ;
2019-06-11 18:20:23 +00:00
d_code_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
2019-03-18 15:28:49 +00:00
d_current_correlation_time_s = d_code_period ;
2019-04-09 15:36:05 +00:00
// Initialize tracking ==========================================
d_carrier_loop_filter . set_params ( trk_parameters . fll_bw_hz , trk_parameters . pll_bw_hz , trk_parameters . pll_filter_order ) ;
2019-03-18 15:28:49 +00:00
d_code_loop_filter . set_noise_bandwidth ( trk_parameters . dll_bw_hz ) ;
d_code_loop_filter . set_update_interval ( d_code_period ) ;
2019-04-09 15:36:05 +00:00
// DLL/PLL filter initialization
d_carrier_loop_filter . initialize ( static_cast < float > ( d_acq_carrier_doppler_hz ) ) ; // initialize the carrier filter
d_code_loop_filter . initialize ( ) ; // initialize the code filter
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-03-18 15:28:49 +00:00
d_pull_in_transitory = true ;
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . clear ( ) ;
2019-06-14 10:52:46 +00:00
d_corrected_doppler = false ;
2018-02-12 16:17:39 +00:00
}
dll_pll_veml_tracking : : ~ dll_pll_veml_tracking ( )
{
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
2019-03-18 07:43:38 +00:00
{
save_matfile ( ) ;
}
2019-02-10 20:55:51 +00:00
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 ( ) ;
}
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-04-25 06:50:21 +00:00
d_Prompt_buffer [ d_cn0_estimation_counter % trk_parameters . cn0_samples ] = d_P_accu ;
d_cn0_estimation_counter + + ;
2019-01-28 01:29:43 +00:00
// Code lock indicator
2019-06-30 13:15:27 +00:00
float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator ( d_Prompt_buffer . data ( ) , trk_parameters . cn0_samples , static_cast < float > ( coh_integration_time_s ) ) ;
2019-04-25 06:50:21 +00:00
d_CN0_SNV_dB_Hz = d_cn0_smoother . smooth ( d_CN0_SNV_dB_Hz_raw ) ;
2019-01-28 01:29:43 +00:00
// Carrier lock indicator
2019-06-30 13:15:27 +00:00
d_carrier_lock_test = d_carrier_lock_test_smoother . smooth ( carrier_lock_detector ( d_Prompt_buffer . data ( ) , 1 ) ) ;
2019-01-28 01:29:43 +00:00
// Loss of lock detection
2019-03-18 15:28:49 +00:00
if ( ! d_pull_in_transitory )
2019-01-28 01:29:43 +00:00
{
2019-06-11 18:20:23 +00:00
if ( d_carrier_lock_test < d_carrier_lock_threshold )
2019-03-18 15:28:49 +00:00
{
d_carrier_lock_fail_counter + + ;
}
else
2018-02-12 16:17:39 +00:00
{
2019-03-18 15:28:49 +00:00
if ( d_carrier_lock_fail_counter > 0 )
{
d_carrier_lock_fail_counter - - ;
}
2018-02-12 16:17:39 +00:00
}
2019-06-11 18:20:23 +00:00
if ( d_CN0_SNV_dB_Hz < trk_parameters . cn0_min )
{
d_code_lock_fail_counter + + ;
}
else
{
if ( d_code_lock_fail_counter > 0 )
{
d_code_lock_fail_counter - - ;
}
}
2019-01-28 01:29:43 +00:00
}
2019-06-11 18:20:23 +00:00
if ( d_carrier_lock_fail_counter > trk_parameters . max_carrier_lock_fail or d_code_lock_fail_counter > trk_parameters . max_code_lock_fail )
2019-01-28 01:29:43 +00:00
{
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! " < < std : : endl ;
2019-06-11 18:20:23 +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 < < " ) " ;
2019-01-28 01:29:43 +00:00
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; // 3 -> loss of lock
d_carrier_lock_fail_counter = 0 ;
2019-06-11 18:20:23 +00:00
d_code_lock_fail_counter = 0 ;
2019-01-28 01:29:43 +00:00
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
2019-03-18 15:28:49 +00:00
d_carr_phase_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
2019-03-18 15:28:49 +00:00
d_carr_phase_error_hz = pll_four_quadrant_atan ( d_P_accu ) / PI_2 ;
2018-02-12 16:17:39 +00:00
}
2019-03-18 15:28:49 +00:00
if ( ( d_pull_in_transitory = = true and trk_parameters . enable_fll_pull_in = = true ) or trk_parameters . enable_fll_steady_state )
{
// FLL discriminator
2019-07-01 09:44:04 +00:00
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI;
2019-05-15 15:38:45 +00:00
d_carr_freq_error_hz = fll_diff_atan ( d_P_accu_old , d_P_accu , 0 , d_current_correlation_time_s ) / GPS_TWO_PI ;
2019-03-18 15:28:49 +00:00
d_P_accu_old = d_P_accu ;
2019-07-01 09:44:04 +00:00
// std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl;
2019-03-18 15:28:49 +00:00
// Carrier discriminator filter
if ( ( d_pull_in_transitory = = true and trk_parameters . enable_fll_pull_in = = true ) )
{
2019-07-01 09:44:04 +00:00
// pure FLL, disable PLL
2019-03-18 15:28:49 +00:00
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( d_carr_freq_error_hz , 0 , d_current_correlation_time_s ) ;
}
else
{
2019-07-01 09:44:04 +00:00
// FLL-aided PLL
2019-03-18 15:28:49 +00:00
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( d_carr_freq_error_hz , d_carr_phase_error_hz , d_current_correlation_time_s ) ;
}
}
else
{
// Carrier discriminator filter
d_carr_error_filt_hz = d_carrier_loop_filter . get_carrier_error ( 0 , d_carr_phase_error_hz , d_current_correlation_time_s ) ;
}
2018-08-03 10:05:40 +00:00
2019-03-18 15:28:49 +00:00
// New carrier Doppler frequency estimation
d_carrier_doppler_hz = d_carr_error_filt_hz ;
2018-02-12 16:17:39 +00:00
2019-03-18 15:28:49 +00:00
// std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl;
// std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl;
2019-07-01 09:44:04 +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
2019-03-18 15:28:49 +00:00
d_code_error_filt_chips = d_code_loop_filter . apply ( 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 ;
2019-06-14 10:52:46 +00:00
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
2019-07-05 18:30:34 +00:00
if ( trk_parameters . enable_doppler_correction = = true )
2019-06-14 10:52:46 +00:00
{
2019-07-05 18:30:34 +00:00
if ( d_pull_in_transitory = = false and d_corrected_doppler = = false )
2019-06-14 10:52:46 +00:00
{
2019-07-05 18:30:34 +00:00
d_dll_filt_history . push_back ( static_cast < float > ( d_code_error_filt_chips ) ) ;
if ( d_dll_filt_history . full ( ) )
2019-06-14 10:52:46 +00:00
{
2019-07-05 18:30:34 +00:00
float avg_code_error_chips_s = std : : accumulate ( d_dll_filt_history . begin ( ) , d_dll_filt_history . end ( ) , 0.0 ) / static_cast < float > ( d_dll_filt_history . capacity ( ) ) ;
if ( fabs ( avg_code_error_chips_s ) > 1.0 )
{
float carrier_doppler_error_hz = static_cast < float > ( d_signal_carrier_freq ) * avg_code_error_chips_s / static_cast < float > ( d_code_chip_rate ) ;
LOG ( INFO ) < < " Detected and corrected carrier doppler error: " < < carrier_doppler_error_hz < < " [Hz] on sat " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) ;
d_carrier_loop_filter . initialize ( d_carrier_doppler_hz - carrier_doppler_error_hz ) ;
d_corrected_doppler = true ;
}
d_dll_filt_history . clear ( ) ;
2019-06-14 10:52:46 +00:00
}
}
}
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 ) ;
2019-07-01 09:00:38 +00:00
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
2019-02-11 20:13:02 +00:00
}
2019-03-18 15:28:49 +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-02-12 16:17:39 +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-01 09:00:38 +00:00
d_current_data_symbol = 0 ;
2019-02-28 13:10:44 +00:00
d_Prompt_circular_buffer . clear ( ) ;
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 > ( 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
}
}
2019-07-01 09:44:04 +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
2019-07-01 09:44:04 +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 ;
}
2019-07-01 09:00:38 +00:00
2019-07-01 09:44:04 +00:00
// data secondary code roll-up
2019-07-01 09:00:38 +00:00
if ( d_symbols_per_bit > 1 )
{
if ( d_data_secondary_code_length > 0 )
{
if ( trk_parameters . track_pilot )
{
if ( d_data_secondary_code_string - > at ( d_current_data_symbol ) = = ' 0 ' )
{
d_P_data_accu + = * d_Prompt_Data ;
}
else
{
d_P_data_accu - = * d_Prompt_Data ;
}
}
else
{
if ( d_data_secondary_code_string - > at ( d_current_data_symbol ) = = ' 0 ' )
{
d_P_data_accu + = * d_Prompt ;
}
else
{
d_P_data_accu - = * d_Prompt ;
}
}
2019-07-03 16:57:26 +00:00
2019-07-01 09:00:38 +00:00
d_current_data_symbol + + ;
// data secondary code roll-up
d_current_data_symbol % = d_data_secondary_code_length ;
}
else
{
if ( trk_parameters . track_pilot )
{
d_P_data_accu + = * d_Prompt_Data ;
}
else
{
d_P_data_accu + = * d_Prompt ;
2019-07-03 16:57:26 +00:00
//std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl;
2019-07-01 09:00:38 +00:00
}
d_current_data_symbol + + ;
d_current_data_symbol % = d_symbols_per_bit ;
}
}
else
{
if ( trk_parameters . track_pilot )
{
d_P_data_accu = * d_Prompt_Data ;
}
else
{
d_P_data_accu = * d_Prompt ;
}
}
2018-04-05 16:08:49 +00:00
if ( trk_parameters . track_pilot )
2019-02-11 20:13:02 +00:00
{
2019-07-01 09:00:38 +00:00
// If tracking pilot, disable Costas loop
2019-03-18 07:43:38 +00:00
d_cloop = false ;
2019-02-11 20:13:02 +00:00
}
2018-03-19 10:19:55 +00:00
else
2019-02-11 20:13:02 +00:00
{
2019-03-18 07:43:38 +00:00
d_cloop = true ;
2019-02-11 20:13:02 +00:00
}
2018-02-12 16:17:39 +00:00
}
2019-07-01 09:44:04 +00:00
2019-07-01 09:00:38 +00:00
void dll_pll_veml_tracking : : log_data ( )
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
{
2019-07-01 09:00:38 +00:00
prompt_I = d_Prompt_Data - > real ( ) ;
prompt_Q = d_Prompt_Data - > imag ( ) ;
2018-03-13 11:38:33 +00:00
}
else
{
2019-07-01 09:00:38 +00:00
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 ) ;
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
2019-03-18 15:28:49 +00:00
tmp_float = d_carr_phase_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 ;
}
2019-07-01 09:44:04 +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-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 ;
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
{
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-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
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-03-29 07:23:23 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2018-03-16 11:21:13 +00:00
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-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
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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-09-08 21:20:29 +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-03-29 07:23:23 +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-09-10 19:29:44 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +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-03-29 07:23:23 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
}
Mat_Close ( matfp ) ;
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 ( ) ;
}
}
}
}
2019-07-01 09:44:04 +00:00
2018-03-29 07:23:23 +00:00
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 ;
}
2019-07-01 09:44:04 +00:00
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 ;
}
2019-07-01 09:44:04 +00:00
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 ( ) ;
2019-07-01 09:00:38 +00:00
current_synchro_data . Flag_valid_symbol_output = false ;
2018-03-29 07:23:23 +00:00
2019-03-18 15:28:49 +00:00
if ( d_pull_in_transitory = = true )
{
if ( trk_parameters . pull_in_time_s < ( d_sample_counter - d_acq_sample_stamp ) / static_cast < int > ( trk_parameters . fs_in ) )
{
d_pull_in_transitory = false ;
2019-06-11 18:20:23 +00:00
d_carrier_lock_fail_counter = 0 ;
d_code_lock_fail_counter = 0 ;
2019-03-18 15:28:49 +00:00
}
}
2018-03-29 07:23:23 +00:00
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 ;
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
2019-04-25 13:30:11 +00:00
d_cn0_smoother . reset ( ) ;
2019-06-11 18:20:23 +00:00
d_carrier_lock_test_smoother . reset ( ) ;
2018-11-29 16:09:30 +00:00
2019-06-14 10:52:46 +00:00
LOG ( INFO ) < < " Number of samples between Acquisition and Tracking = " < < acq_trk_diff_samples < < " ( " < < acq_trk_diff_seconds < < " s) " ;
2018-11-29 16:09:30 +00:00
DLOG ( INFO ) < < " PULL-IN Doppler [Hz] = " < < d_carrier_doppler_hz
< < " . PULL-IN Code Phase [samples] = " < < d_acq_code_phase_samples ;
2019-04-25 13:30:11 +00:00
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
2019-07-09 09:52:53 +00:00
//fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period
2019-07-03 16:57:26 +00:00
if ( trk_parameters . bit_synchronization_time_limit_s < ( d_sample_counter - d_acq_sample_stamp ) / static_cast < int > ( trk_parameters . fs_in ) )
{
d_carrier_lock_fail_counter = 300000 ; //force loss-of-lock condition
LOG ( INFO ) < < systemName < < " " < < signal_pretty_name < < " tracking synchronization time limit reached in channel " < < d_channel
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
}
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)
2019-07-01 09:00:38 +00:00
log_data ( ) ;
2019-06-11 18:20:23 +00:00
if ( ! d_pull_in_transitory )
2018-03-13 11:38:33 +00:00
{
2019-06-11 18:20:23 +00:00
if ( d_secondary )
2018-03-16 11:21:13 +00:00
{
2019-06-11 18:20:23 +00:00
// ####### SECONDARY CODE LOCK #####
d_Prompt_circular_buffer . push_back ( * d_Prompt ) ;
if ( d_Prompt_circular_buffer . size ( ) = = d_secondary_code_length )
2018-03-29 07:23:23 +00:00
{
2019-06-11 18:20:23 +00:00
next_state = acquire_secondary ( ) ;
if ( next_state )
{
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 ;
std : : cout < < systemName < < " " < < signal_pretty_name < < " secondary code locked in channel " < < d_channel
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
}
2018-03-29 07:23:23 +00:00
}
}
2019-06-11 18:20:23 +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
{
2019-07-01 09:00:38 +00:00
//******* preamble correlation ********
d_Prompt_circular_buffer . push_back ( * d_Prompt ) ;
if ( d_Prompt_circular_buffer . size ( ) = = d_secondary_code_length )
2018-03-29 07:23:23 +00:00
{
2019-07-01 09:00:38 +00:00
next_state = acquire_secondary ( ) ;
if ( next_state )
2019-06-11 18:20:23 +00:00
{
2019-07-01 09:00:38 +00:00
LOG ( INFO ) < < systemName < < " " < < signal_pretty_name < < " tracking bit synchronization locked in channel " < < d_channel
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
std : : cout < < systemName < < " " < < signal_pretty_name < < " tracking bit synchronization locked in channel " < < d_channel
2019-06-11 18:20:23 +00:00
< < " for satellite " < < Gnss_Satellite ( systemName , d_acquisition_gnss_synchro - > PRN ) < < std : : endl ;
}
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
{
2019-06-11 18:20:23 +00:00
next_state = true ;
2018-07-13 13:23:20 +00:00
}
}
else
{
2019-06-11 18:20:23 +00:00
next_state = false ; //keep in state 2 during pull-in transitory
2018-07-13 13:23:20 +00:00
}
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 ) ;
2019-07-01 09:00:38 +00:00
d_P_data_accu = gr_complex ( 0.0 , 0.0 ) ;
2018-03-13 11:38:33 +00:00
d_L_accu = gr_complex ( 0.0 , 0.0 ) ;
d_VL_accu = 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 ;
2019-07-01 09:00:38 +00:00
d_current_data_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 ;
2019-03-18 15:28:49 +00:00
d_current_correlation_time_s = static_cast < float > ( trk_parameters . extend_correlation_symbols ) * static_cast < float > ( d_code_period ) ;
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]
2019-03-18 15:28:49 +00:00
d_code_loop_filter . set_update_interval ( d_current_correlation_time_s ) ;
d_code_loop_filter . set_noise_bandwidth ( trk_parameters . dll_bw_narrow_hz ) ;
d_carrier_loop_filter . set_params ( trk_parameters . fll_bw_hz , trk_parameters . pll_bw_narrow_hz , trk_parameters . pll_filter_order ) ;
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)
{
// perform a correlation step
do_correlation_step ( in ) ;
2018-03-13 11:38:33 +00:00
save_correlation_results ( ) ;
2019-07-01 09:00:38 +00:00
update_tracking_vars ( ) ;
if ( d_current_data_symbol = = 0 )
2018-03-12 14:16:39 +00:00
{
2019-07-03 16:57:26 +00:00
log_data ( ) ;
2019-07-01 09:00:38 +00:00
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
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 ( ) ) ;
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 ) ;
2018-03-12 14:16:39 +00:00
}
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 ;
}
break ;
}
case 4 : // narrow tracking
{
// 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 ( ) ;
2019-07-01 09:00:38 +00:00
if ( d_current_data_symbol = = 0 )
2018-03-13 11:38:33 +00:00
{
2019-07-03 16:57:26 +00:00
// enable write dump file this cycle (valid DLL/PLL cycle)
log_data ( ) ;
2019-07-01 09:00:38 +00:00
// ########### Output the tracking results to Telemetry block ##########
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
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 ( ) ) ;
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 ) ;
2018-03-13 11:38:33 +00:00
}
2019-07-03 16:57:26 +00:00
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
}