2015-07-23 16:07:52 +00:00
/*!
* \ file gps_l1_ca_dll_pll_tracking_gpu_cc . cc
2015-12-11 12:53:43 +00:00
* \ brief Implementation of a code DLL + carrier PLL tracking block GPU ACCELERATED
2015-07-23 16:07:52 +00:00
* \ author Javier Arribas , 2015. jarribas ( at ) cttc . es
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2015-07-23 16:07:52 +00:00
*
2020-07-28 14:57:15 +00:00
* Copyright ( C ) 2010 - 2020 ( see AUTHORS file for a list of contributors )
2015-07-23 16:07:52 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2015-07-23 16:07:52 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2015-07-23 16:07:52 +00:00
*/
# include "gps_l1_ca_dll_pll_tracking_gpu_cc.h"
# include "GPS_L1_CA.h"
2020-04-25 14:08:33 +00:00
# include "gnss_satellite.h"
2018-02-18 12:55:04 +00:00
# include "gnss_sdr_flags.h"
2018-12-09 21:00:09 +00:00
# include "gps_sdr_signal_processing.h"
# include "lock_detectors.h"
# include "tracking_discriminators.h"
2018-04-29 02:16:56 +00:00
# include <boost/lexical_cast.hpp>
# include <glog/logging.h>
2018-12-09 21:00:09 +00:00
# include <gnuradio/io_signature.h>
2018-04-29 02:16:56 +00:00
# include <cmath>
2018-12-09 21:00:09 +00:00
# include <cuda_profiler_api.h>
2018-04-29 02:16:56 +00:00
# include <iostream>
# include <memory>
# include <sstream>
2015-09-10 15:46:38 +00:00
2015-07-23 16:07:52 +00:00
gps_l1_ca_dll_pll_tracking_gpu_cc_sptr
gps_l1_ca_dll_pll_make_tracking_gpu_cc (
2018-08-10 18:34:03 +00:00
int64_t fs_in ,
2018-08-12 22:54:23 +00:00
uint32_t vector_length ,
2018-03-03 01:03:39 +00:00
bool dump ,
std : : string dump_filename ,
float pll_bw_hz ,
float dll_bw_hz ,
float early_late_space_chips )
2015-07-23 16:07:52 +00:00
{
2018-06-06 15:25:03 +00:00
return gps_l1_ca_dll_pll_tracking_gpu_cc_sptr ( new Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc (
2018-03-03 01:03:39 +00:00
fs_in , vector_length , dump , dump_filename , pll_bw_hz , dll_bw_hz , early_late_space_chips ) ) ;
2015-07-23 16:07:52 +00:00
}
2018-03-03 01:03:39 +00:00
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : forecast ( int noutput_items ,
gr_vector_int & ninput_items_required )
2015-07-23 16:07:52 +00:00
{
2015-12-11 12:53:43 +00:00
if ( noutput_items ! = 0 )
{
2019-08-18 20:16:13 +00:00
ninput_items_required [ 0 ] = static_cast < int32_t > ( d_vector_length ) * 2 ; // set the required available samples in each call
2015-12-11 12:53:43 +00:00
}
2015-07-23 16:07:52 +00:00
}
Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc (
2018-08-10 18:34:03 +00:00
int64_t fs_in ,
2018-08-12 22:54:23 +00:00
uint32_t vector_length ,
2018-03-03 01:03:39 +00:00
bool dump ,
std : : string dump_filename ,
float pll_bw_hz ,
float dll_bw_hz ,
float early_late_space_chips ) : gr : : block ( " Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc " , gr : : io_signature : : make ( 1 , 1 , sizeof ( gr_complex ) ) ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) )
2015-07-23 16:07:52 +00:00
{
2016-03-30 20:27:12 +00:00
// Telemetry bit synchronization message port input
this - > message_port_register_in ( pmt : : mp ( " preamble_timestamp_s " ) ) ;
2016-04-21 16:54:08 +00:00
this - > message_port_register_out ( pmt : : mp ( " events " ) ) ;
2019-03-18 20:35:55 +00:00
this - > message_port_register_in ( pmt : : mp ( " telemetry_to_trk " ) ) ;
2015-07-23 16:07:52 +00:00
// initialize internal vars
d_dump = dump ;
d_fs_in = fs_in ;
d_vector_length = vector_length ;
d_dump_filename = dump_filename ;
2018-08-12 22:54:23 +00:00
d_correlation_length_samples = static_cast < int32_t > ( d_vector_length ) ;
2015-07-23 16:07:52 +00:00
// Initialize tracking ==========================================
d_code_loop_filter . set_DLL_BW ( dll_bw_hz ) ;
2018-03-03 01:03:39 +00:00
d_carrier_loop_filter . set_params ( 10.0 , pll_bw_hz , 2 ) ;
2015-07-23 16:07:52 +00:00
2019-08-18 20:16:13 +00:00
// --- DLL variables -------------------------------------------------------
2018-03-03 01:03:39 +00:00
d_early_late_spc_chips = early_late_space_chips ; // Define early-late offset (in chips)
2015-07-23 16:07:52 +00:00
2015-11-16 18:23:25 +00:00
// Set GPU flags
cudaSetDeviceFlags ( cudaDeviceMapHost ) ;
2019-08-18 20:16:13 +00:00
// allocate host memory
// pinned memory mode - use special function to get OS-pinned memory
2018-03-03 01:03:39 +00:00
d_n_correlator_taps = 3 ; // Early, Prompt, and Late
2015-11-16 18:23:25 +00:00
// Get space for a vector with the C/A code replica sampled 1x/chip
2019-08-18 10:54:16 +00:00
cudaHostAlloc ( reinterpret_cast < void * * > ( & d_ca_code ) , ( static_cast < int32_t > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) * sizeof ( gr_complex ) ) , cudaHostAllocMapped | | cudaHostAllocWriteCombined ) ;
2015-11-16 18:23:25 +00:00
// Get space for the resampled early / prompt / late local replicas
2019-08-18 10:54:16 +00:00
cudaHostAlloc ( reinterpret_cast < void * * > ( & d_local_code_shift_chips ) , d_n_correlator_taps * sizeof ( float ) , cudaHostAllocMapped | | cudaHostAllocWriteCombined ) ;
cudaHostAlloc ( reinterpret_cast < void * * > ( & in_gpu ) , 2 * d_vector_length * sizeof ( gr_complex ) , cudaHostAllocMapped | | cudaHostAllocWriteCombined ) ;
2015-11-30 12:59:31 +00:00
// correlator outputs (scalar)
2019-08-18 10:54:16 +00:00
cudaHostAlloc ( reinterpret_cast < void * * > ( & d_correlator_outs ) , sizeof ( gr_complex ) * d_n_correlator_taps , cudaHostAllocMapped | | cudaHostAllocWriteCombined ) ;
2015-12-11 12:53:43 +00:00
// Set TAPs delay values [chips]
2018-03-03 01:03:39 +00:00
d_local_code_shift_chips [ 0 ] = - d_early_late_spc_chips ;
2015-12-11 12:53:43 +00:00
d_local_code_shift_chips [ 1 ] = 0.0 ;
d_local_code_shift_chips [ 2 ] = d_early_late_spc_chips ;
2015-09-10 15:46:38 +00:00
2019-08-18 20:16:13 +00:00
// --- Perform initializations ------------------------------
2015-11-16 18:23:25 +00:00
multicorrelator_gpu = new cuda_multicorrelator ( ) ;
2019-08-18 20:16:13 +00:00
// local code resampler on GPU
2015-12-11 12:53:43 +00:00
multicorrelator_gpu - > init_cuda_integrated_resampler ( 2 * d_vector_length , GPS_L1_CA_CODE_LENGTH_CHIPS , d_n_correlator_taps ) ;
multicorrelator_gpu - > set_input_output_vectors ( d_correlator_outs , in_gpu ) ;
2015-11-30 12:59:31 +00:00
2015-07-23 16:07:52 +00:00
// define initial code frequency basis of NCO
2019-08-31 09:37:29 +00:00
d_code_freq_chips = GPS_L1_CA_CODE_RATE_CPS ;
2015-07-23 16:07:52 +00:00
// define residual code phase (in chips)
d_rem_code_phase_samples = 0.0 ;
// define residual carrier phase
2015-12-11 12:53:43 +00:00
d_rem_carrier_phase_rad = 0.0 ;
2015-07-23 16:07:52 +00:00
// sample synchronization
2018-08-12 22:54:23 +00:00
d_sample_counter = 0ULL ;
2019-08-18 20:16:13 +00:00
// d_sample_counter_seconds = 0;
2015-07-23 16:07:52 +00:00
d_acq_sample_stamp = 0 ;
d_enable_tracking = false ;
d_pull_in = false ;
// 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 > ( FLAGS_cn0_samples ) ;
2015-07-23 16:07:52 +00:00
d_carrier_lock_test = 1 ;
d_CN0_SNV_dB_Hz = 0 ;
d_carrier_lock_fail_counter = 0 ;
2018-02-18 12:55:04 +00:00
d_carrier_lock_threshold = FLAGS_carrier_lock_th ;
2015-07-23 16:07:52 +00:00
systemName [ " G " ] = std : : string ( " GPS " ) ;
systemName [ " S " ] = std : : string ( " SBAS " ) ;
2015-12-11 12:53:43 +00:00
set_relative_rate ( 1.0 / ( static_cast < double > ( d_vector_length ) * 2.0 ) ) ;
2015-07-23 16:07:52 +00:00
d_acquisition_gnss_synchro = 0 ;
d_channel = 0 ;
d_acq_code_phase_samples = 0.0 ;
d_acq_carrier_doppler_hz = 0.0 ;
d_carrier_doppler_hz = 0.0 ;
2015-12-11 12:53:43 +00:00
d_acc_carrier_phase_cycles = 0.0 ;
2015-07-23 16:07:52 +00:00
d_code_phase_samples = 0.0 ;
2015-12-11 12:53:43 +00:00
d_pll_to_dll_assist_secs_Ti = 0.0 ;
d_rem_code_phase_chips = 0.0 ;
d_code_phase_step_chips = 0.0 ;
d_carrier_phase_step_rad = 0.0 ;
2020-07-20 09:27:39 +00:00
d_acc_carrier_phase_initialized = false ;
2019-08-18 20:16:13 +00:00
// set_min_output_buffer((int64_t)300);
2015-07-23 16:07:52 +00:00
}
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : start_tracking ( )
{
/*
* correct the code phase according to the delay between acq and trk
*/
d_acq_code_phase_samples = d_acquisition_gnss_synchro - > Acq_delay_samples ;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro - > Acq_doppler_hz ;
2015-12-11 12:53:43 +00:00
d_acq_sample_stamp = d_acquisition_gnss_synchro - > Acq_samplestamp_samples ;
2015-07-23 16:07:52 +00:00
2020-07-20 09:27:39 +00:00
const int64_t acq_trk_diff_samples = static_cast < int64_t > ( d_sample_counter ) - static_cast < int64_t > ( d_acq_sample_stamp ) ; // -d_vector_length;
2015-07-23 16:07:52 +00:00
DLOG ( INFO ) < < " Number of samples between Acquisition and Tracking = " < < acq_trk_diff_samples ;
2020-07-20 09:27:39 +00:00
const double acq_trk_diff_seconds = static_cast < double > ( acq_trk_diff_samples ) / static_cast < double > ( d_fs_in ) ;
2019-08-18 20:16:13 +00:00
// doppler effect
2015-07-23 16:07:52 +00:00
// Fd=(C/(C+Vr))*F
2020-07-20 09:27:39 +00:00
const double radial_velocity = ( GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz ) / GPS_L1_FREQ_HZ ;
2015-07-23 16:07:52 +00:00
// new chip and prn sequence periods based on acq Doppler
2019-08-31 09:37:29 +00:00
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_CPS ;
2015-12-11 12:53:43 +00:00
d_code_phase_step_chips = static_cast < double > ( d_code_freq_chips ) / static_cast < double > ( d_fs_in ) ;
2020-07-20 09:27:39 +00:00
const double T_chip_mod_seconds = 1 / d_code_freq_chips ;
const double T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS ;
2020-08-27 12:57:44 +00:00
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast < double > ( d_fs_in ) ;
2015-07-23 16:07:52 +00:00
2015-12-11 12:53:43 +00:00
d_correlation_length_samples = round ( T_prn_mod_samples ) ;
2015-07-23 16:07:52 +00:00
2020-07-20 09:27:39 +00:00
const double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_CPS ;
const double T_prn_true_samples = T_prn_true_seconds * static_cast < double > ( d_fs_in ) ;
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds ;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds ;
2015-12-02 18:00:29 +00:00
double corrected_acq_phase_samples , delay_correction_samples ;
corrected_acq_phase_samples = fmod ( ( d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast < double > ( d_fs_in ) ) , T_prn_true_samples ) ;
2015-07-23 16:07:52 +00:00
if ( corrected_acq_phase_samples < 0 )
{
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples ;
}
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples ;
d_acq_code_phase_samples = corrected_acq_phase_samples ;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz ;
2020-07-05 18:20:02 +00:00
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast < double > ( d_fs_in ) ;
2015-12-11 12:53:43 +00:00
2015-07-23 16:07:52 +00:00
// DLL/PLL filter initialization
2019-08-18 20:16:13 +00:00
d_carrier_loop_filter . initialize ( d_acq_carrier_doppler_hz ) ; // The carrier loop filter implements the Doppler accumulator
2018-03-03 01:03:39 +00:00
d_code_loop_filter . initialize ( ) ; // initialize the code filter
2015-07-23 16:07:52 +00:00
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
2020-05-07 21:11:11 +00:00
gps_l1_ca_code_gen_complex ( own : : span < gr_complex > ( d_ca_code , static_cast < int32_t > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) ) , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2015-08-06 15:05:15 +00:00
2018-08-12 22:54:23 +00:00
multicorrelator_gpu - > set_local_code_and_taps ( static_cast < int32_t > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) , d_ca_code , d_local_code_shift_chips , d_n_correlator_taps ) ;
2015-08-06 15:05:15 +00:00
2018-08-12 22:54:23 +00:00
for ( int32_t n = 0 ; n < d_n_correlator_taps ; n + + )
2015-12-11 12:53:43 +00:00
{
2018-03-03 01:03:39 +00:00
d_correlator_outs [ n ] = gr_complex ( 0 , 0 ) ;
2015-12-11 12:53:43 +00:00
}
2015-07-23 16:07:52 +00:00
d_carrier_lock_fail_counter = 0 ;
2015-12-11 12:53:43 +00:00
d_rem_code_phase_samples = 0.0 ;
d_rem_carrier_phase_rad = 0.0 ;
d_rem_code_phase_chips = 0.0 ;
d_acc_carrier_phase_cycles = 0.0 ;
d_pll_to_dll_assist_secs_Ti = 0.0 ;
2015-07-23 16:07:52 +00:00
d_code_phase_samples = d_acq_code_phase_samples ;
2020-07-20 09:27:39 +00:00
const std : : string sys_ = & d_acquisition_gnss_synchro - > System ;
2018-03-03 01:03:39 +00:00
sys = sys_ . substr ( 0 , 1 ) ;
2015-07-23 16:07:52 +00:00
// DEBUG OUTPUT
2020-07-07 16:53:50 +00:00
std : : cout < < " Tracking of GPS L1 C/A signal started on channel " < < d_channel < < " for satellite " < < Gnss_Satellite ( systemName [ sys ] , d_acquisition_gnss_synchro - > PRN ) < < ' \n ' ;
2018-04-10 23:22:11 +00:00
LOG ( INFO ) < < " Tracking of GPS L1 C/A signal for satellite " < < Gnss_Satellite ( systemName [ sys ] , d_acquisition_gnss_synchro - > PRN ) < < " on channel " < < d_channel ;
2015-07-23 16:07:52 +00:00
// enable tracking
d_pull_in = true ;
d_enable_tracking = true ;
2020-07-20 09:27:39 +00:00
d_acc_carrier_phase_initialized = false ;
2015-07-23 16:07:52 +00:00
LOG ( INFO ) < < " PULL-IN Doppler [Hz]= " < < d_carrier_doppler_hz
2017-07-25 14:50:22 +00:00
< < " Code Phase correction [samples]= " < < delay_correction_samples
< < " PULL-IN Code Phase [samples]= " < < d_acq_code_phase_samples ;
2015-07-23 16:07:52 +00:00
}
Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : ~ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc ( )
{
2017-07-25 14:50:22 +00:00
if ( d_dump_file . is_open ( ) )
{
try
2018-03-03 01:03:39 +00:00
{
2017-07-25 14:50:22 +00:00
d_dump_file . close ( ) ;
2018-03-03 01:03:39 +00:00
}
catch ( const std : : exception & ex )
{
2017-07-25 14:50:22 +00:00
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
2018-03-03 01:03:39 +00:00
}
2017-07-25 14:50:22 +00:00
}
2018-03-03 01:03:39 +00:00
try
{
2017-07-25 14:50:22 +00:00
cudaFreeHost ( in_gpu ) ;
cudaFreeHost ( d_correlator_outs ) ;
cudaFreeHost ( d_local_code_shift_chips ) ;
cudaFreeHost ( d_ca_code ) ;
multicorrelator_gpu - > free_cuda ( ) ;
2018-03-03 01:03:39 +00:00
delete ( multicorrelator_gpu ) ;
}
catch ( const std : : exception & ex )
{
2017-07-25 14:50:22 +00:00
LOG ( WARNING ) < < " Exception in destructor " < < ex . what ( ) ;
2018-03-03 01:03:39 +00:00
}
2015-07-23 16:07:52 +00:00
}
2018-08-12 22:54:23 +00:00
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : set_channel ( uint32_t channel )
2018-04-10 23:22:11 +00:00
{
d_channel = channel ;
LOG ( INFO ) < < " Tracking Channel set to " < < d_channel ;
// ############# ENABLE DATA FILE LOG #################
if ( d_dump = = true )
{
if ( d_dump_file . is_open ( ) = = false )
{
try
{
d_dump_filename . append ( boost : : lexical_cast < std : : string > ( d_channel ) ) ;
d_dump_filename . append ( " .dat " ) ;
d_dump_file . exceptions ( std : : ifstream : : failbit | std : : ifstream : : badbit ) ;
d_dump_file . open ( d_dump_filename . c_str ( ) , std : : ios : : out | std : : ios : : binary ) ;
LOG ( INFO ) < < " Tracking dump enabled on channel " < < d_channel < < " Log file: " < < d_dump_filename . c_str ( ) ;
}
catch ( const std : : ifstream : : failure * e )
{
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening trk dump file " < < e - > what ( ) ;
}
}
}
}
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : set_gnss_synchro ( Gnss_Synchro * p_gnss_synchro )
{
d_acquisition_gnss_synchro = p_gnss_synchro ;
}
2020-07-20 09:27:39 +00:00
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : check_carrier_phase_coherent_initialization ( )
{
if ( d_acc_carrier_phase_initialized = = false )
{
d_acc_carrier_phase_cycles = - d_rem_carrier_phase_rad / TWO_PI ;
d_acc_carrier_phase_initialized = true ;
}
}
2018-03-03 01:03:39 +00:00
int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
2015-07-23 16:07:52 +00:00
{
// Block input data and block output stream pointers
2018-03-03 01:03:39 +00:00
const gr_complex * in = reinterpret_cast < const gr_complex * > ( input_items [ 0 ] ) ;
2017-08-19 00:33:54 +00:00
Gnss_Synchro * * out = reinterpret_cast < Gnss_Synchro * * > ( & output_items [ 0 ] ) ;
2015-07-23 16:07:52 +00:00
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro ( ) ;
2015-12-11 12:53:43 +00:00
// process vars
double code_error_chips_Ti = 0.0 ;
double code_error_filt_chips = 0.0 ;
double code_error_filt_secs_Ti = 0.0 ;
2019-06-25 09:40:10 +00:00
double CURRENT_INTEGRATION_TIME_S = 0.001 ;
double CORRECTED_INTEGRATION_TIME_S = 0.001 ;
2015-12-11 12:53:43 +00:00
double dll_code_error_secs_Ti = 0.0 ;
double carr_phase_error_secs_Ti = 0.0 ;
2020-10-09 18:55:31 +00:00
bool loss_of_lock = false ;
2015-07-23 16:07:52 +00:00
if ( d_enable_tracking = = true )
{
2016-05-02 21:46:30 +00:00
// Fill the acquisition data
2016-04-07 16:25:45 +00:00
current_synchro_data = * d_acquisition_gnss_synchro ;
2015-07-23 16:07:52 +00:00
// Receiver signal alignment
if ( d_pull_in = = true )
{
2020-07-20 09:27:39 +00:00
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp ;
const double acq_trk_shif_correction_samples = d_correlation_length_samples - fmod ( static_cast < double > ( acq_to_trk_delay_samples ) , static_cast < double > ( d_correlation_length_samples ) ) ;
const int32_t samples_offset = round ( d_acq_code_phase_samples + acq_trk_shif_correction_samples ) ;
2018-08-12 22:54:23 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + static_cast < uint64_t > ( samples_offset ) ;
2017-06-12 15:49:08 +00:00
current_synchro_data . fs = d_fs_in ;
2017-04-27 13:28:30 +00:00
current_synchro_data . correlation_length_ms = 1 ;
2016-04-07 16:25:45 +00:00
* out [ 0 ] = current_synchro_data ;
2019-08-18 20:16:13 +00:00
d_sample_counter + = static_cast < uint64_t > ( samples_offset ) ; // count for the processed samples
2015-07-23 16:07:52 +00:00
d_pull_in = false ;
2019-08-18 20:16:13 +00:00
consume_each ( samples_offset ) ; // shift input to perform alignment with local replica
2015-07-23 16:07:52 +00:00
return 1 ;
}
2015-12-11 12:53:43 +00:00
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
2015-08-06 15:05:15 +00:00
2015-12-11 12:53:43 +00:00
memcpy ( in_gpu , in , sizeof ( gr_complex ) * d_correlation_length_samples ) ;
2015-08-06 15:05:15 +00:00
cudaProfilerStart ( ) ;
2018-03-03 01:03:39 +00:00
multicorrelator_gpu - > Carrier_wipeoff_multicorrelator_resampler_cuda ( static_cast < float > ( d_rem_carrier_phase_rad ) ,
static_cast < float > ( d_carrier_phase_step_rad ) ,
static_cast < float > ( d_code_phase_step_chips ) ,
static_cast < float > ( d_rem_code_phase_chips ) ,
d_correlation_length_samples , d_n_correlator_taps ) ;
2015-08-25 17:01:02 +00:00
cudaProfilerStop ( ) ;
2020-07-07 16:53:50 +00:00
// std::cout<<"c_out[0]="<<d_correlator_outs[0]<<"c_out[1]="<<d_correlator_outs[1]<<"c_out[2]="<<d_correlator_outs[2]<< '\n';
2015-12-11 12:53:43 +00:00
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast < double > ( d_correlation_length_samples ) / static_cast < double > ( d_fs_in ) ;
2015-07-23 16:07:52 +00:00
// ################## PLL ##########################################################
2015-12-11 12:53:43 +00:00
// Update PLL discriminator [rads/Ti -> Secs/Ti]
2020-07-05 18:20:02 +00:00
carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan ( d_correlator_outs [ 1 ] ) / TWO_PI ; // prompt output
2015-07-23 16:07:52 +00:00
// Carrier discriminator filter
2015-12-11 12:53:43 +00:00
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
2019-08-18 20:16:13 +00:00
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
2015-12-11 12:53:43 +00:00
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter . get_carrier_error ( 0.0 , carr_phase_error_secs_Ti , CURRENT_INTEGRATION_TIME_S ) ;
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = ( d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S ) / GPS_L1_FREQ_HZ ;
// code Doppler frequency update
2019-08-31 09:37:29 +00:00
d_code_freq_chips = GPS_L1_CA_CODE_RATE_CPS + ( ( d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_CPS ) / GPS_L1_FREQ_HZ ) ;
2015-07-23 16:07:52 +00:00
// ################## DLL ##########################################################
// DLL discriminator
2019-11-08 17:37:56 +00:00
code_error_chips_Ti = dll_nc_e_minus_l_normalized ( d_correlator_outs [ 0 ] , d_correlator_outs [ 2 ] , d_early_late_spc_chips , 1.0 ) ; // [chips/Ti] // early and late
2015-07-23 16:07:52 +00:00
// Code discriminator filter
2019-08-18 20:16:13 +00:00
code_error_filt_chips = d_code_loop_filter . get_code_nco ( code_error_chips_Ti ) ; // input [chips/Ti] -> output [chips/second]
2018-03-03 01:03:39 +00:00
code_error_filt_secs_Ti = code_error_filt_chips * CURRENT_INTEGRATION_TIME_S / d_code_freq_chips ; // [s/Ti]
2015-12-11 12:53:43 +00:00
// DLL code error estimation [s/Ti]
// TODO: PLL carrier aid to DLL is disabled. Re-enable it and measure performance
2018-03-03 01:03:39 +00:00
dll_code_error_secs_Ti = - code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti ;
2015-07-23 16:07:52 +00:00
2018-03-25 17:47:28 +00:00
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
2015-07-23 16:07:52 +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
2020-07-20 09:27:39 +00:00
const double T_chip_seconds = 1 / d_code_freq_chips ;
const double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS ;
const double T_prn_samples = T_prn_seconds * static_cast < double > ( d_fs_in ) ;
const double K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast < double > ( d_fs_in ) ;
2015-12-11 12:53:43 +00:00
2019-08-18 20:16:13 +00:00
d_correlation_length_samples = round ( K_blk_samples ) ; // round to a discrete samples
d_rem_code_phase_samples = K_blk_samples - static_cast < double > ( d_correlation_length_samples ) ; // rounding error < 1 sample
2015-12-11 12:53:43 +00:00
// UPDATE REMNANT CARRIER PHASE
2018-03-03 01:03:39 +00:00
CORRECTED_INTEGRATION_TIME_S = ( static_cast < double > ( d_correlation_length_samples ) / static_cast < double > ( d_fs_in ) ) ;
2019-08-18 20:16:13 +00:00
// remnant carrier phase [rad]
2020-07-05 18:20:02 +00:00
d_rem_carrier_phase_rad = fmod ( d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S , TWO_PI ) ;
2015-12-11 12:53:43 +00:00
// UPDATE CARRIER PHASE ACCUULATOR
2019-08-18 20:16:13 +00:00
// carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
2015-12-11 12:53:43 +00:00
d_acc_carrier_phase_cycles - = d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S ;
2019-08-18 20:16:13 +00:00
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
2020-07-05 18:20:02 +00:00
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast < double > ( d_fs_in ) ;
2015-12-11 12:53:43 +00:00
2019-08-18 20:16:13 +00:00
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
2015-12-11 12:53:43 +00:00
d_code_phase_step_chips = d_code_freq_chips / static_cast < double > ( d_fs_in ) ;
2019-08-18 20:16:13 +00:00
// remnant code phase [chips]
2015-12-11 12:53:43 +00:00
d_rem_code_phase_chips = d_rem_code_phase_samples * ( d_code_freq_chips / static_cast < double > ( d_fs_in ) ) ;
// ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
2018-02-18 12:55:04 +00:00
if ( d_cn0_estimation_counter < FLAGS_cn0_samples )
2015-07-23 16:07:52 +00:00
{
// fill buffer with prompt correlator output values
2019-08-18 20:16:13 +00:00
d_Prompt_buffer [ d_cn0_estimation_counter ] = d_correlator_outs [ 1 ] ; // prompt
2015-07-23 16:07:52 +00:00
d_cn0_estimation_counter + + ;
}
else
{
d_cn0_estimation_counter = 0 ;
// Code lock indicator
2019-09-27 22:39:14 +00:00
d_CN0_SNV_dB_Hz = cn0_m2m4_estimator ( d_Prompt_buffer . data ( ) , FLAGS_cn0_samples , GPS_L1_CA_CODE_PERIOD_S ) ;
2015-07-23 16:07:52 +00:00
// Carrier lock indicator
2019-06-30 13:15:27 +00:00
d_carrier_lock_test = carrier_lock_detector ( d_Prompt_buffer . data ( ) , FLAGS_cn0_samples ) ;
2015-07-23 16:07:52 +00:00
// Loss of lock detection
2018-02-18 12:55:04 +00:00
if ( d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min )
2015-07-23 16:07:52 +00:00
{
d_carrier_lock_fail_counter + + ;
}
else
{
if ( d_carrier_lock_fail_counter > 0 ) d_carrier_lock_fail_counter - - ;
}
2018-02-18 12:55:04 +00:00
if ( d_carrier_lock_fail_counter > FLAGS_max_lock_fail )
2015-07-23 16:07:52 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! \n " ;
2015-07-23 16:07:52 +00:00
LOG ( INFO ) < < " Loss of lock in channel " < < d_channel < < " ! " ;
2019-08-18 20:16:13 +00:00
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; // 3 -> loss of lock
2015-07-23 16:07:52 +00:00
d_carrier_lock_fail_counter = 0 ;
2018-03-03 01:03:39 +00:00
d_enable_tracking = false ; // TODO: check if disabling tracking is consistent with the channel state machine
2020-10-09 18:55:31 +00:00
loss_of_lock = true ;
2015-07-23 16:07:52 +00:00
}
2020-07-20 09:27:39 +00:00
check_carrier_phase_coherent_initialization ( ) ;
2015-07-23 16:07:52 +00:00
}
2015-12-11 12:53:43 +00:00
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data . Prompt_I = static_cast < double > ( ( d_correlator_outs [ 1 ] ) . real ( ) ) ;
current_synchro_data . Prompt_Q = static_cast < double > ( ( d_correlator_outs [ 1 ] ) . imag ( ) ) ;
2018-08-12 22:54:23 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + static_cast < uint64_t > ( d_correlation_length_samples ) ;
2017-04-12 15:04:51 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
2020-07-05 18:20:02 +00:00
current_synchro_data . Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles ;
2015-12-02 18:00:29 +00:00
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
2020-10-09 18:55:31 +00:00
current_synchro_data . Flag_valid_symbol_output = ! loss_of_lock ;
2018-03-03 01:03:39 +00:00
current_synchro_data . correlation_length_ms = 1 ;
2015-07-23 16:07:52 +00:00
}
else
{
2018-08-12 22:54:23 +00:00
for ( int32_t n = 0 ; n < d_n_correlator_taps ; n + + )
2015-12-11 12:53:43 +00:00
{
2018-03-03 01:03:39 +00:00
d_correlator_outs [ n ] = gr_complex ( 0 , 0 ) ;
2015-12-11 12:53:43 +00:00
}
2015-07-23 16:07:52 +00:00
current_synchro_data . System = { ' G ' } ;
2017-04-27 13:28:30 +00:00
current_synchro_data . correlation_length_ms = 1 ;
2018-08-12 22:54:23 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + static_cast < uint64_t > ( d_correlation_length_samples ) ;
2015-07-23 16:07:52 +00:00
}
2019-08-18 20:16:13 +00:00
// assign the GNU Radio block output data
2017-06-12 15:49:08 +00:00
current_synchro_data . fs = d_fs_in ;
2016-04-07 16:25:45 +00:00
* out [ 0 ] = current_synchro_data ;
2018-03-03 01:03:39 +00:00
if ( d_dump )
2015-07-23 16:07:52 +00:00
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I ;
float prompt_Q ;
float tmp_E , tmp_P , tmp_L ;
2018-03-29 15:53:25 +00:00
float tmp_VE = 0.0 ;
float tmp_VL = 0.0 ;
float tmp_float ;
2015-12-11 12:53:43 +00:00
prompt_I = d_correlator_outs [ 1 ] . real ( ) ;
prompt_Q = d_correlator_outs [ 1 ] . imag ( ) ;
tmp_E = std : : abs < float > ( d_correlator_outs [ 0 ] ) ;
tmp_P = std : : abs < float > ( d_correlator_outs [ 1 ] ) ;
tmp_L = std : : abs < float > ( d_correlator_outs [ 2 ] ) ;
2015-07-23 16:07:52 +00:00
try
2018-03-03 01:03:39 +00:00
{
2018-03-29 15:53:25 +00:00
// Dump correlators output
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VE ) , sizeof ( float ) ) ;
2018-03-03 01:03:39 +00:00
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 ) ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VL ) , sizeof ( float ) ) ;
2015-12-11 12:53:43 +00:00
// PROMPT I and Q (to analyze navigation symbols)
2018-03-03 01:03: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 ) ) ;
2015-12-11 12:53:43 +00:00
// PRN start sample stamp
2018-08-10 18:34:03 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & d_sample_counter ) , sizeof ( uint64_t ) ) ;
2015-12-11 12:53:43 +00:00
// accumulated carrier phase
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_acc_carrier_phase_cycles * TWO_PI ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2015-12-11 12:53:43 +00:00
// carrier and code frequency
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_doppler_hz ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_code_freq_chips ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// PLL commands
2019-06-25 09:40:10 +00:00
tmp_float = 1.0 / ( carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2019-06-25 09:40:10 +00:00
tmp_float = 1.0 / ( code_error_filt_secs_Ti * CURRENT_INTEGRATION_TIME_S ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// DLL commands
2019-06-25 09:40:10 +00:00
tmp_float = code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2019-06-25 09:40:10 +00:00
tmp_float = code_error_filt_secs_Ti ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2015-12-11 12:53:43 +00:00
// CN0 and carrier lock test
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_CN0_SNV_dB_Hz ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_lock_test ) ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2015-12-11 12:53:43 +00:00
// AUX vars (for debug purposes)
2019-06-25 09:40:10 +00:00
tmp_float = code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S ;
2018-03-29 15:53:25 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
double tmp_double = static_cast < double > ( d_sample_counter + d_correlation_length_samples ) ;
2018-03-03 01:03:39 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
2017-06-16 19:24:14 +00:00
// PRN
2018-08-12 22:54:23 +00:00
uint32_t prn_ = d_acquisition_gnss_synchro - > PRN ;
d_dump_file . write ( reinterpret_cast < char * > ( & prn_ ) , sizeof ( uint32_t ) ) ;
2018-03-03 01:03:39 +00:00
}
catch ( const std : : ifstream : : failure * e )
{
2015-12-11 12:53:43 +00:00
LOG ( WARNING ) < < " Exception writing trk dump file " < < e - > what ( ) ;
2018-03-03 01:03:39 +00:00
}
2015-07-23 16:07:52 +00:00
}
2018-03-03 01:03:39 +00:00
consume_each ( d_correlation_length_samples ) ; // this is necessary in gr::block derivates
2019-08-18 20:16:13 +00:00
d_sample_counter + = d_correlation_length_samples ; // count for the processed samples
2015-12-11 12:53:43 +00:00
2020-10-09 18:55:31 +00:00
if ( d_enable_tracking | | loss_of_lock )
2018-03-03 01:03:39 +00:00
{
return 1 ;
}
else
{
return 0 ;
}
2015-07-23 16:07:52 +00:00
}