2018-03-13 09:51:33 +00:00
/*!
2022-08-30 09:30:53 +00:00
* \ file gps_l1_ca_gaussian_tracking_cc . cc
2018-04-10 08:52:19 +00:00
* \ brief Implementation of a processing block of a DLL + Kalman carrier
* tracking loop for GPS L1 C / A signals
* \ author Javier Arribas , 2018. jarribas ( at ) cttc . es
* \ author Jordi Vila - Valls 2018. jvila ( at ) cttc . es
* \ author Carles Fernandez - Prades 2018. cfernandez ( at ) cttc . es
2018-03-13 09:51:33 +00:00
*
2018-04-10 08:52:19 +00:00
* Reference :
* J . Vila - Valls , P . Closas , M . Navarro and C . Fernandez - Prades ,
* " Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Carrier Synchronization " , IEEE Aerospace and Electronic Systems Magazine,
* Vol . 32 , No . 7 , pp . 28 – 45 , July 2017. DOI : 10.1109 / MAES .2017 .150260
2018-03-13 09:51:33 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2018-03-13 09:51:33 +00:00
*
2020-12-30 12:35:06 +00:00
* GNSS - SDR is a Global Navigation Satellite System software - defined receiver .
2018-03-13 09:51:33 +00:00
* This file is part of GNSS - SDR .
*
2020-12-30 12:35:06 +00:00
* Copyright ( C ) 2010 - 2020 ( see AUTHORS file for a list of contributors )
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2018-03-13 09:51:33 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2018-03-13 09:51:33 +00:00
*/
2022-08-30 09:30:53 +00:00
# include "gps_l1_ca_gaussian_tracking_cc.h"
2018-03-13 09:51:33 +00:00
# include "GPS_L1_CA.h"
2020-04-25 14:08:33 +00:00
# include "gnss_satellite.h"
2018-12-09 21:00:09 +00:00
# include "gnss_sdr_flags.h"
2020-11-07 21:43:19 +00:00
# include "gps_sdr_signal_replica.h"
2018-12-09 21:00:09 +00:00
# include "lock_detectors.h"
# include "tracking_discriminators.h"
2018-03-15 17:56:29 +00:00
# include <glog/logging.h>
2018-12-09 21:00:09 +00:00
# include <gnuradio/io_signature.h>
2018-03-15 17:56:29 +00:00
# include <matio.h>
2018-12-09 21:00:09 +00:00
# include <volk_gnsssdr/volk_gnsssdr.h>
2019-10-26 20:24:42 +00:00
# include <algorithm>
2019-07-14 12:09:12 +00:00
# include <array>
2018-03-15 17:56:29 +00:00
# include <cmath>
2019-02-10 20:55:51 +00:00
# include <exception>
2018-03-15 17:56:29 +00:00
# include <iostream>
# include <memory>
# include <sstream>
2018-12-03 18:01:47 +00:00
# include <utility>
2019-10-26 20:24:42 +00:00
# include <vector>
2018-03-13 09:51:33 +00:00
2022-08-30 09:30:53 +00:00
gps_l1_ca_gaussian_tracking_cc_sptr gps_l1_ca_gaussian_make_tracking_cc (
2018-08-21 13:00:57 +00:00
uint32_t order ,
int64_t fs_in ,
uint32_t vector_length ,
2018-03-15 17:56:29 +00:00
bool dump ,
2019-09-11 22:31:34 +00:00
const std : : string & dump_filename ,
2018-03-15 17:56:29 +00:00
float dll_bw_hz ,
2018-08-14 01:18:01 +00:00
float early_late_space_chips ,
bool bce_run ,
2018-08-21 13:00:57 +00:00
uint32_t bce_ptrans ,
uint32_t bce_strans ,
int32_t bce_nu ,
int32_t bce_kappa )
2018-03-13 09:51:33 +00:00
{
2022-08-30 09:30:53 +00:00
return gps_l1_ca_gaussian_tracking_cc_sptr ( new Gps_L1_Ca_Gaussian_Tracking_cc ( order ,
2019-09-13 06:56:37 +00:00
fs_in , vector_length , dump , dump_filename , dll_bw_hz , early_late_space_chips ,
2018-08-14 01:18:01 +00:00
bce_run , bce_ptrans , bce_strans , bce_nu , bce_kappa ) ) ;
2018-03-13 09:51:33 +00:00
}
2022-08-30 09:30:53 +00:00
void Gps_L1_Ca_Gaussian_Tracking_cc : : forecast ( int noutput_items ,
2018-03-15 17:56:29 +00:00
gr_vector_int & ninput_items_required )
2018-03-13 09:51:33 +00:00
{
if ( noutput_items ! = 0 )
{
2018-08-21 13:20:48 +00:00
ninput_items_required [ 0 ] = static_cast < int > ( d_vector_length ) * 2 ; // set the required available samples in each call
2018-03-13 09:51:33 +00:00
}
}
2022-08-30 09:30:53 +00:00
Gps_L1_Ca_Gaussian_Tracking_cc : : Gps_L1_Ca_Gaussian_Tracking_cc (
2018-08-21 13:00:57 +00:00
uint32_t order ,
int64_t fs_in ,
uint32_t vector_length ,
2018-03-15 17:56:29 +00:00
bool dump ,
2019-09-11 22:31:34 +00:00
const std : : string & dump_filename ,
2018-03-15 17:56:29 +00:00
float dll_bw_hz ,
2018-08-14 01:18:01 +00:00
float early_late_space_chips ,
bool bce_run ,
2018-08-21 13:00:57 +00:00
uint32_t bce_ptrans ,
uint32_t bce_strans ,
int32_t bce_nu ,
2021-12-15 01:48:18 +00:00
int32_t bce_kappa )
2022-08-30 09:30:53 +00:00
: gr : : block ( " Gps_L1_Ca_Gaussian_Tracking_cc " , gr : : io_signature : : make ( 1 , 1 , sizeof ( gr_complex ) ) ,
2021-12-15 01:48:18 +00:00
gr : : io_signature : : make ( 1 , 1 , sizeof ( Gnss_Synchro ) ) ) ,
d_order ( order ) ,
d_vector_length ( vector_length ) ,
d_dump ( dump ) ,
d_acquisition_gnss_synchro ( nullptr ) ,
d_channel ( 0 ) ,
d_fs_in ( fs_in ) ,
d_early_late_spc_chips ( early_late_space_chips ) ,
d_rem_code_phase_samples ( 0.0 ) ,
d_rem_code_phase_chips ( 0.0 ) ,
d_rem_carr_phase_rad ( 0.0 ) ,
bayes_ptrans ( bce_ptrans ) ,
bayes_strans ( bce_strans ) ,
bayes_nu ( bce_nu ) ,
bayes_kappa ( bce_kappa ) ,
bayes_run ( bce_run ) ,
kf_iter ( 0 ) ,
d_acq_code_phase_samples ( 0.0 ) ,
d_acq_carrier_doppler_hz ( 0.0 ) ,
d_n_correlator_taps ( 3 ) ,
d_code_freq_chips ( GPS_L1_CA_CODE_RATE_CPS ) ,
d_code_phase_step_chips ( 0.0 ) ,
d_code_phase_rate_step_chips ( 0.0 ) ,
d_carrier_doppler_hz ( 0.0 ) ,
d_carrier_dopplerrate_hz2 ( 0.0 ) ,
d_carrier_phase_step_rad ( 0.0 ) ,
d_acc_carrier_phase_rad ( 0.0 ) ,
d_carr_phase_sigma2 ( 0.0 ) ,
d_code_phase_samples ( 0.0 ) ,
code_error_chips ( 0.0 ) ,
code_error_filt_chips ( 0.0 ) ,
d_current_prn_length_samples ( static_cast < int > ( d_vector_length ) ) ,
d_sample_counter ( 0 ) ,
d_acq_sample_stamp ( 0 ) ,
d_cn0_estimation_counter ( 0 ) ,
d_carrier_lock_test ( 1 ) ,
d_CN0_SNV_dB_Hz ( 0 ) ,
d_carrier_lock_threshold ( FLAGS_carrier_lock_th ) ,
d_carrier_lock_fail_counter ( 0 ) ,
d_enable_tracking ( false ) ,
d_pull_in ( false ) ,
d_dump_filename ( dump_filename )
2018-03-13 09:51:33 +00:00
{
// Telemetry bit synchronization message port input
this - > message_port_register_in ( pmt : : mp ( " preamble_timestamp_s " ) ) ;
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 " ) ) ;
2018-03-13 09:51:33 +00:00
d_code_loop_filter . set_DLL_BW ( dll_bw_hz ) ;
// Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip
2021-12-15 01:48:18 +00:00
d_ca_code = volk_gnsssdr : : vector < float > ( static_cast < int > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) ) ;
2018-03-13 09:51:33 +00:00
2021-12-15 01:48:18 +00:00
d_correlator_outs = volk_gnsssdr : : vector < gr_complex > ( d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2019-10-26 20:24:42 +00:00
2021-10-01 20:35:12 +00:00
d_local_code_shift_chips = volk_gnsssdr : : vector < float > ( d_n_correlator_taps ) ;
2018-03-13 09:51:33 +00:00
// Set TAPs delay values [chips]
2018-03-15 17:56:29 +00:00
d_local_code_shift_chips [ 0 ] = - d_early_late_spc_chips ;
2018-03-13 09:51:33 +00:00
d_local_code_shift_chips [ 1 ] = 0.0 ;
d_local_code_shift_chips [ 2 ] = d_early_late_spc_chips ;
multicorrelator_cpu . init ( 2 * d_current_prn_length_samples , d_n_correlator_taps ) ;
2021-10-01 20:35:12 +00:00
d_Prompt_buffer = volk_gnsssdr : : vector < gr_complex > ( FLAGS_cn0_samples ) ;
2018-03-13 09:51:33 +00:00
systemName [ " G " ] = std : : string ( " GPS " ) ;
systemName [ " S " ] = std : : string ( " SBAS " ) ;
2022-12-03 23:11:42 +00:00
# if GNURADIO_GREATER_THAN_38
this - > set_relative_rate ( 1 , static_cast < uint64_t > ( d_vector_length ) ) ;
# else
this - > set_relative_rate ( 1.0 / static_cast < double > ( d_vector_length ) ) ;
# endif
2018-03-13 09:51:33 +00:00
// Kalman filter initialization (receiver initialization)
2021-12-15 01:48:18 +00:00
const double CN_dB_Hz = 30 ;
const double CN_lin = pow ( 10 , CN_dB_Hz / 10.0 ) ;
2018-03-13 09:51:33 +00:00
2021-12-15 01:48:18 +00:00
const double sigma2_phase_detector_cycles2 = ( 1.0 / ( 2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S ) ) * ( 1.0 + 1.0 / ( 2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S ) ) ;
2018-03-13 09:51:33 +00:00
2018-08-21 13:20:48 +00:00
// covariances (static)
2021-12-15 01:48:18 +00:00
const double sigma2_carrier_phase = TWO_PI / 4 ;
const double sigma2_doppler = 450 ;
const double sigma2_doppler_rate = pow ( 4.0 * TWO_PI , 2 ) / 12.0 ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_P_x_ini = arma : : zeros ( 2 , 2 ) ;
kf_P_x_ini ( 0 , 0 ) = sigma2_carrier_phase ;
kf_P_x_ini ( 1 , 1 ) = sigma2_doppler ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_R = arma : : zeros ( 1 , 1 ) ;
kf_R ( 0 , 0 ) = sigma2_phase_detector_cycles2 ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_Q = arma : : zeros ( 2 , 2 ) ;
2019-08-31 09:37:29 +00:00
kf_Q ( 0 , 0 ) = pow ( GPS_L1_CA_CODE_PERIOD_S , 4 ) ;
kf_Q ( 1 , 1 ) = GPS_L1_CA_CODE_PERIOD_S ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_F = arma : : zeros ( 2 , 2 ) ;
kf_F ( 0 , 0 ) = 1.0 ;
2020-07-05 18:20:02 +00:00
kf_F ( 0 , 1 ) = TWO_PI * GPS_L1_CA_CODE_PERIOD_S ;
2018-03-15 17:56:29 +00:00
kf_F ( 1 , 0 ) = 0.0 ;
kf_F ( 1 , 1 ) = 1.0 ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_H = arma : : zeros ( 1 , 2 ) ;
kf_H ( 0 , 0 ) = 1.0 ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
kf_x = arma : : zeros ( 2 , 1 ) ;
kf_y = arma : : zeros ( 1 , 1 ) ;
2018-08-14 01:15:58 +00:00
kf_P_y = arma : : zeros ( 1 , 1 ) ;
2018-07-03 18:47:17 +00:00
// order three
if ( d_order = = 3 )
{
kf_P_x_ini = arma : : resize ( kf_P_x_ini , 3 , 3 ) ;
kf_P_x_ini ( 2 , 2 ) = sigma2_doppler_rate ;
kf_Q = arma : : zeros ( 3 , 3 ) ;
2019-08-31 09:37:29 +00:00
kf_Q ( 0 , 0 ) = pow ( GPS_L1_CA_CODE_PERIOD_S , 4 ) ;
kf_Q ( 1 , 1 ) = GPS_L1_CA_CODE_PERIOD_S ;
kf_Q ( 2 , 2 ) = GPS_L1_CA_CODE_PERIOD_S ;
2018-07-03 18:47:17 +00:00
kf_F = arma : : resize ( kf_F , 3 , 3 ) ;
2020-07-05 18:20:02 +00:00
kf_F ( 0 , 2 ) = 0.5 * TWO_PI * pow ( GPS_L1_CA_CODE_PERIOD_S , 2 ) ;
2019-08-31 09:37:29 +00:00
kf_F ( 1 , 2 ) = GPS_L1_CA_CODE_PERIOD_S ;
2018-07-03 18:47:17 +00:00
kf_F ( 2 , 0 ) = 0.0 ;
kf_F ( 2 , 1 ) = 0.0 ;
kf_F ( 2 , 2 ) = 1.0 ;
kf_H = arma : : resize ( kf_H , 1 , 3 ) ;
kf_H ( 0 , 2 ) = 0.0 ;
kf_x = arma : : resize ( kf_x , 3 , 1 ) ;
2018-08-14 01:15:58 +00:00
kf_x ( 2 , 0 ) = 0.0 ;
2018-07-03 18:47:17 +00:00
}
2018-03-13 09:51:33 +00:00
2022-08-30 09:30:53 +00:00
// Gaussian covariance estimator initialization
2018-08-21 13:00:57 +00:00
kf_R_est = kf_R ;
2018-08-14 01:18:01 +00:00
2018-08-21 13:00:57 +00:00
bayes_estimator . init ( arma : : zeros ( 1 , 1 ) , bayes_kappa , bayes_nu , ( kf_H * kf_P_x_ini * kf_H . t ( ) + kf_R ) * ( bayes_nu + 2 ) ) ;
2018-08-14 01:15:58 +00:00
}
2018-03-13 09:51:33 +00:00
2019-08-18 20:16:13 +00:00
2022-08-30 09:30:53 +00:00
void Gps_L1_Ca_Gaussian_Tracking_cc : : start_tracking ( )
2018-03-13 09:51:33 +00:00
{
/*
* 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 ;
d_acq_sample_stamp = d_acquisition_gnss_synchro - > Acq_samplestamp_samples ;
2018-07-03 17:31:53 +00:00
d_acq_carrier_doppler_step_hz = static_cast < double > ( d_acquisition_gnss_synchro - > Acq_doppler_step ) ;
// Correct Kalman filter covariance according to acq doppler step size (3 sigma)
if ( d_acquisition_gnss_synchro - > Acq_doppler_step > 0 )
{
2018-08-14 01:15:58 +00:00
kf_P_x_ini ( 1 , 1 ) = pow ( d_acq_carrier_doppler_step_hz / 3.0 , 2 ) ;
2018-08-21 13:00:57 +00:00
bayes_estimator . init ( arma : : zeros ( 1 , 1 ) , bayes_kappa , bayes_nu , ( kf_H * kf_P_x_ini * kf_H . t ( ) + kf_R ) * ( bayes_nu + 2 ) ) ;
2018-07-03 17:31:53 +00:00
}
2018-03-13 09:51:33 +00:00
2018-08-21 13:00:57 +00:00
int64_t acq_trk_diff_samples ;
2018-03-13 09:51:33 +00:00
double acq_trk_diff_seconds ;
2019-08-18 20:16:13 +00:00
acq_trk_diff_samples = static_cast < int64_t > ( d_sample_counter ) - static_cast < int64_t > ( d_acq_sample_stamp ) ; // -d_vector_length;
2018-03-13 09:51:33 +00:00
DLOG ( INFO ) < < " Number of samples between Acquisition and Tracking = " < < acq_trk_diff_samples ;
acq_trk_diff_seconds = static_cast < float > ( acq_trk_diff_samples ) / static_cast < float > ( d_fs_in ) ;
2018-04-10 08:52:19 +00:00
// Doppler effect Fd = (C / (C + Vr)) * F
2018-03-13 09:51:33 +00:00
double radial_velocity = ( GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz ) / GPS_L1_FREQ_HZ ;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds ;
double T_prn_mod_seconds ;
double T_prn_mod_samples ;
2019-08-31 09:37:29 +00:00
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_CPS ;
2018-03-13 09:51:33 +00:00
d_code_phase_step_chips = static_cast < double > ( d_code_freq_chips ) / static_cast < double > ( d_fs_in ) ;
2018-03-15 17:56:29 +00:00
T_chip_mod_seconds = 1 / d_code_freq_chips ;
2018-03-13 09:51:33 +00:00
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS ;
T_prn_mod_samples = T_prn_mod_seconds * static_cast < double > ( d_fs_in ) ;
d_current_prn_length_samples = round ( T_prn_mod_samples ) ;
2019-08-31 09:37:29 +00:00
double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_CPS ;
2018-03-13 09:51:33 +00:00
double T_prn_true_samples = T_prn_true_seconds * static_cast < double > ( d_fs_in ) ;
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds ;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds ;
2019-08-12 22:19:31 +00:00
double corrected_acq_phase_samples ;
double delay_correction_samples ;
2018-03-13 09:51:33 +00:00
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 ) ;
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 ;
2018-08-14 01:15:58 +00:00
d_carrier_dopplerrate_hz2 = 0 ;
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 ) ;
2018-03-13 09:51:33 +00:00
2018-03-15 17:56:29 +00:00
// DLL filter initialization
d_code_loop_filter . initialize ( ) ; // initialize the code filter
2018-03-13 09:51:33 +00:00
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
2019-10-26 20:24:42 +00:00
gps_l1_ca_code_gen_float ( d_ca_code , d_acquisition_gnss_synchro - > PRN , 0 ) ;
2018-03-13 09:51:33 +00:00
2019-10-26 20:24:42 +00:00
multicorrelator_cpu . set_local_code_and_taps ( static_cast < int > ( GPS_L1_CA_CODE_LENGTH_CHIPS ) , d_ca_code . data ( ) , d_local_code_shift_chips . data ( ) ) ;
std : : fill_n ( d_correlator_outs . begin ( ) , d_n_correlator_taps , gr_complex ( 0.0 , 0.0 ) ) ;
2018-03-13 09:51:33 +00:00
d_carrier_lock_fail_counter = 0 ;
d_rem_code_phase_samples = 0 ;
d_rem_carr_phase_rad = 0.0 ;
d_rem_code_phase_chips = 0.0 ;
d_acc_carrier_phase_rad = 0.0 ;
2018-08-14 01:18:01 +00:00
d_carr_phase_sigma2 = 0.0 ;
2018-03-13 09:51:33 +00:00
d_code_phase_samples = d_acq_code_phase_samples ;
2022-02-16 12:56:25 +00:00
sys = std : : string ( 1 , d_acquisition_gnss_synchro - > System ) ;
2022-03-25 08:14:28 +00:00
2018-03-13 09:51:33 +00:00
// enable tracking
d_pull_in = true ;
d_enable_tracking = true ;
LOG ( INFO ) < < " PULL-IN Doppler [Hz]= " < < d_carrier_doppler_hz
2018-03-15 17:56:29 +00:00
< < " Code Phase correction [samples]= " < < delay_correction_samples
< < " PULL-IN Code Phase [samples]= " < < d_acq_code_phase_samples ;
2023-12-21 12:57:41 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
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 ' ;
LOG ( INFO ) < < " Starting tracking of satellite " < < Gnss_Satellite ( systemName [ sys ] , d_acquisition_gnss_synchro - > PRN ) < < " on channel " < < d_channel ;
2018-03-13 09:51:33 +00:00
}
2022-08-30 09:30:53 +00:00
Gps_L1_Ca_Gaussian_Tracking_cc : : ~ Gps_L1_Ca_Gaussian_Tracking_cc ( )
2018-03-13 09:51:33 +00:00
{
if ( d_dump_file . is_open ( ) )
{
try
2018-03-15 17:56:29 +00:00
{
2018-03-13 09:51:33 +00:00
d_dump_file . close ( ) ;
2018-03-15 17:56:29 +00:00
}
catch ( const std : : exception & ex )
{
2019-10-26 20:24:42 +00:00
LOG ( WARNING ) < < " Exception in Tracking block destructor: " < < ex . what ( ) ;
2018-03-15 17:56:29 +00:00
}
}
if ( d_dump )
{
if ( d_channel = = 0 )
{
std : : cout < < " Writing .mat files ... " ;
}
2019-02-10 20:55:51 +00:00
try
{
2022-08-30 09:30:53 +00:00
Gps_L1_Ca_Gaussian_Tracking_cc : : 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-03-15 17:56:29 +00:00
if ( d_channel = = 0 )
{
2020-07-07 16:53:50 +00:00
std : : cout < < " done. \n " ;
2018-03-15 17:56:29 +00:00
}
2018-03-13 09:51:33 +00:00
}
try
2018-03-15 17:56:29 +00:00
{
2018-03-13 09:51:33 +00:00
multicorrelator_cpu . free ( ) ;
2018-03-15 17:56:29 +00:00
}
catch ( const std : : exception & ex )
{
2019-10-26 20:24:42 +00:00
LOG ( WARNING ) < < " Exception in Tracking block destructor: " < < ex . what ( ) ;
2018-03-15 17:56:29 +00:00
}
2018-03-13 09:51:33 +00:00
}
2022-08-30 09:30:53 +00:00
int32_t Gps_L1_Ca_Gaussian_Tracking_cc : : save_matfile ( )
2018-03-15 17:56:29 +00:00
{
// READ DUMP FILE
std : : ifstream : : pos_type size ;
2018-08-21 13:00:57 +00:00
int32_t number_of_double_vars = 1 ;
int32_t number_of_float_vars = 19 ;
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-15 17:56:29 +00:00
std : : ifstream dump_file ;
dump_file . exceptions ( std : : ifstream : : failbit | std : : ifstream : : badbit ) ;
try
{
dump_file . open ( d_dump_filename . c_str ( ) , std : : ios : : binary | std : : ios : : ate ) ;
}
catch ( const std : : ifstream : : failure & e )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " Problem opening dump file: " < < e . what ( ) < < ' \n ' ;
2018-03-15 17:56:29 +00:00
return 1 ;
}
// count number of epochs and rewind
2018-08-21 13:00:57 +00:00
int64_t num_epoch = 0 ;
2018-03-15 17:56:29 +00:00
if ( dump_file . is_open ( ) )
{
size = dump_file . tellg ( ) ;
2018-08-21 13:00:57 +00:00
num_epoch = static_cast < int64_t > ( size ) / static_cast < int64_t > ( epoch_size_bytes ) ;
2018-03-15 17:56:29 +00:00
dump_file . seekg ( 0 , std : : ios : : beg ) ;
}
else
{
return 1 ;
}
2019-06-30 12:32:40 +00:00
auto abs_VE = std : : vector < float > ( num_epoch ) ;
auto abs_E = std : : vector < float > ( num_epoch ) ;
auto abs_P = std : : vector < float > ( num_epoch ) ;
auto abs_L = std : : vector < float > ( num_epoch ) ;
auto abs_VL = std : : vector < float > ( num_epoch ) ;
auto Prompt_I = std : : vector < float > ( num_epoch ) ;
auto Prompt_Q = std : : vector < float > ( num_epoch ) ;
auto PRN_start_sample_count = std : : vector < uint64_t > ( num_epoch ) ;
auto acc_carrier_phase_rad = std : : vector < float > ( num_epoch ) ;
auto carrier_doppler_hz = std : : vector < float > ( num_epoch ) ;
auto carrier_dopplerrate_hz2 = std : : vector < float > ( num_epoch ) ;
auto code_freq_chips = std : : vector < float > ( num_epoch ) ;
auto carr_error_hz = std : : vector < float > ( num_epoch ) ;
auto carr_noise_sigma2 = std : : vector < float > ( num_epoch ) ;
auto carr_error_filt_hz = std : : vector < float > ( num_epoch ) ;
2020-02-26 21:40:00 +00:00
auto code_error_chips_aux = std : : vector < float > ( num_epoch ) ;
auto code_error_filt_chips_aux = std : : vector < float > ( num_epoch ) ;
2019-06-30 12:32:40 +00:00
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 < int32_t > ( num_epoch ) ;
2018-03-15 17:56:29 +00:00
try
{
if ( dump_file . is_open ( ) )
{
2018-08-21 13:00:57 +00:00
for ( int64_t i = 0 ; i < num_epoch ; i + + )
2018-03-15 17:56:29 +00:00
{
2018-04-03 14:37:36 +00:00
dump_file . read ( reinterpret_cast < char * > ( & abs_VE [ i ] ) , sizeof ( float ) ) ;
2018-03-15 17:56:29 +00:00
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 ) ) ;
2018-04-03 14:37:36 +00:00
dump_file . read ( reinterpret_cast < char * > ( & abs_VL [ i ] ) , sizeof ( float ) ) ;
2018-03-15 17:56:29 +00:00
dump_file . read ( reinterpret_cast < char * > ( & Prompt_I [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & Prompt_Q [ i ] ) , sizeof ( float ) ) ;
2018-08-21 13:00:57 +00:00
dump_file . read ( reinterpret_cast < char * > ( & PRN_start_sample_count [ i ] ) , sizeof ( uint64_t ) ) ;
2018-04-03 14:37:36 +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-08-14 01:15:58 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carrier_dopplerrate_hz2 [ i ] ) , sizeof ( float ) ) ;
2018-04-03 14:37:36 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_freq_chips [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & carr_error_hz [ i ] ) , sizeof ( float ) ) ;
2018-08-14 01:18:01 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carr_noise_sigma2 [ i ] ) , sizeof ( float ) ) ;
2018-04-03 14:37:36 +00:00
dump_file . read ( reinterpret_cast < char * > ( & carr_error_filt_hz [ i ] ) , sizeof ( float ) ) ;
2020-02-26 21:40:00 +00:00
dump_file . read ( reinterpret_cast < char * > ( & code_error_chips_aux [ i ] ) , sizeof ( float ) ) ;
dump_file . read ( reinterpret_cast < char * > ( & code_error_filt_chips_aux [ i ] ) , sizeof ( float ) ) ;
2018-04-03 14:37:36 +00:00
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 ) ) ;
2018-03-15 17:56:29 +00:00
dump_file . read ( reinterpret_cast < char * > ( & aux2 [ i ] ) , sizeof ( double ) ) ;
2018-08-21 13:00:57 +00:00
dump_file . read ( reinterpret_cast < char * > ( & PRN [ i ] ) , sizeof ( uint32_t ) ) ;
2018-03-15 17:56:29 +00:00
}
}
dump_file . close ( ) ;
}
catch ( const std : : ifstream : : failure & e )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " Problem reading dump file: " < < e . what ( ) < < ' \n ' ;
2018-03-15 17:56:29 +00:00
return 1 ;
}
// WRITE MAT FILE
mat_t * matfp ;
matvar_t * matvar ;
std : : string filename = d_dump_filename ;
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 ) ;
2018-12-08 17:49:31 +00:00
if ( reinterpret_cast < int64_t * > ( matfp ) ! = nullptr )
2018-03-15 17:56:29 +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-04-03 14:37:36 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2019-07-14 12:09:12 +00:00
matvar = Mat_VarCreate ( " abs_E " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_E . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " abs_P " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_P . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " abs_L " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_L . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " abs_VL " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , abs_VL . data ( ) , 0 ) ;
2018-04-03 14:37:36 +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-15 17:56: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 ( " Prompt_Q " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , Prompt_Q . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " PRN_start_sample_count " , MAT_C_UINT64 , MAT_T_UINT64 , 2 , dims . data ( ) , PRN_start_sample_count . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " acc_carrier_phase_rad " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , acc_carrier_phase_rad . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " carrier_doppler_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_doppler_hz . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " carrier_dopplerrate_hz2 " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_dopplerrate_hz2 . data ( ) , 0 ) ;
2018-08-14 01:15:58 +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-15 17:56: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 ( " carr_error_hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carr_error_hz . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " carr_noise_sigma2 " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carr_noise_sigma2 . data ( ) , 0 ) ;
2018-08-14 01:18:01 +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-15 17:56:29 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2020-02-26 21:40:00 +00:00
matvar = Mat_VarCreate ( " code_error_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_error_chips_aux . data ( ) , 0 ) ;
2018-03-15 17:56:29 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
2020-02-26 21:40:00 +00:00
matvar = Mat_VarCreate ( " code_error_filt_chips " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , code_error_filt_chips_aux . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " CN0_SNV_dB_Hz " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , CN0_SNV_dB_Hz . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " carrier_lock_test " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , carrier_lock_test . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " aux1 " , MAT_C_SINGLE , MAT_T_SINGLE , 2 , dims . data ( ) , aux1 . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " aux2 " , MAT_C_DOUBLE , MAT_T_DOUBLE , 2 , dims . data ( ) , aux2 . data ( ) , 0 ) ;
2018-03-15 17:56: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 ( " PRN " , MAT_C_UINT32 , MAT_T_UINT32 , 2 , dims . data ( ) , PRN . data ( ) , 0 ) ;
2018-03-15 17:56:29 +00:00
Mat_VarWrite ( matfp , matvar , MAT_COMPRESSION_ZLIB ) ; // or MAT_COMPRESSION_NONE
Mat_VarFree ( matvar ) ;
}
Mat_Close ( matfp ) ;
return 0 ;
}
2018-03-13 09:51:33 +00:00
2022-08-30 09:30:53 +00:00
void Gps_L1_Ca_Gaussian_Tracking_cc : : set_channel ( uint32_t channel )
2018-03-13 09:51:33 +00:00
{
2018-04-10 08:52:19 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2018-03-13 09:51:33 +00:00
d_channel = channel ;
LOG ( INFO ) < < " Tracking Channel set to " < < d_channel ;
// ############# ENABLE DATA FILE LOG #################
2018-04-10 08:52:19 +00:00
if ( d_dump )
2018-03-13 09:51:33 +00:00
{
2018-04-10 08:52:19 +00:00
if ( ! d_dump_file . is_open ( ) )
2018-03-13 09:51:33 +00:00
{
try
2018-03-15 17:56:29 +00:00
{
2018-12-03 22:34:29 +00:00
d_dump_filename . append ( std : : to_string ( d_channel ) ) ;
2018-03-13 09:51:33 +00:00
d_dump_filename . append ( " .dat " ) ;
2022-12-11 11:48:00 +00:00
d_dump_file . exceptions ( std : : ofstream : : failbit | std : : ofstream : : badbit ) ;
2018-03-13 09:51:33 +00:00
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 ( ) ;
2018-03-15 17:56:29 +00:00
}
2022-12-11 11:48:00 +00:00
catch ( const std : : ofstream : : failure & e )
2018-03-15 17:56:29 +00:00
{
2018-03-13 09:51:33 +00:00
LOG ( WARNING ) < < " channel " < < d_channel < < " Exception opening trk dump file " < < e . what ( ) ;
2018-03-15 17:56:29 +00:00
}
2018-03-13 09:51:33 +00:00
}
}
}
2022-08-30 09:30:53 +00:00
void Gps_L1_Ca_Gaussian_Tracking_cc : : set_gnss_synchro ( Gnss_Synchro * p_gnss_synchro )
2018-03-13 09:51:33 +00:00
{
d_acquisition_gnss_synchro = p_gnss_synchro ;
}
2018-04-10 08:52:19 +00:00
2022-08-30 09:30:53 +00:00
int Gps_L1_Ca_Gaussian_Tracking_cc : : general_work ( int noutput_items __attribute__ ( ( unused ) ) , gr_vector_int & ninput_items __attribute__ ( ( unused ) ) ,
2018-04-10 08:52:19 +00:00
gr_vector_const_void_star & input_items , gr_vector_void_star & output_items )
{
// process vars
2018-08-14 01:18:01 +00:00
d_carr_phase_error_rad = 0.0 ;
2020-02-26 21:40:00 +00:00
code_error_chips = 0.0 ;
code_error_filt_chips = 0.0 ;
2020-10-09 18:55:31 +00:00
bool loss_of_lock = false ;
2018-04-10 08:52:19 +00:00
// Block input data and block output stream pointers
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-04-10 08:52:19 +00:00
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro ( ) ;
if ( d_enable_tracking = = true )
{
// Fill the acquisition data
current_synchro_data = * d_acquisition_gnss_synchro ;
// Receiver signal alignment
if ( d_pull_in = = true )
{
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
2018-08-21 13:00:57 +00:00
uint64_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp ;
2018-04-10 08:52:19 +00:00
double acq_trk_shif_correction_samples = static_cast < double > ( d_current_prn_length_samples ) - std : : fmod ( static_cast < double > ( acq_to_trk_delay_samples ) , static_cast < double > ( d_current_prn_length_samples ) ) ;
2018-08-21 13:00:57 +00:00
int32_t samples_offset = std : : round ( d_acq_code_phase_samples + acq_trk_shif_correction_samples ) ;
2018-04-10 08:52:19 +00:00
if ( samples_offset < 0 )
{
samples_offset = 0 ;
}
d_acc_carrier_phase_rad - = d_carrier_phase_step_rad * d_acq_code_phase_samples ;
d_sample_counter + = samples_offset ; // count for the processed samples
d_pull_in = false ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . fs = d_fs_in ;
current_synchro_data . correlation_length_ms = 1 ;
2023-11-27 12:41:55 +00:00
* out [ 0 ] = std : : move ( current_synchro_data ) ;
2018-04-10 08:52:19 +00:00
// Kalman filter initialization reset
kf_P_x = kf_P_x_ini ;
// Update Kalman states based on acquisition information
kf_x ( 0 ) = d_carrier_phase_step_rad * samples_offset ;
2018-08-14 01:15:58 +00:00
kf_x ( 1 ) = d_carrier_doppler_hz ;
if ( kf_x . n_elem > 2 )
{
kf_x ( 2 ) = d_carrier_dopplerrate_hz2 ;
}
2018-04-10 08:52:19 +00:00
2018-08-14 01:18:01 +00:00
// Covariance estimation initialization reset
kf_iter = 0 ;
2018-08-21 13:00:57 +00:00
bayes_estimator . init ( arma : : zeros ( 1 , 1 ) , bayes_kappa , bayes_nu , ( kf_H * kf_P_x_ini * kf_H . t ( ) + kf_R ) * ( bayes_nu + 2 ) ) ;
2018-08-14 01:18:01 +00:00
2018-04-10 08:52:19 +00:00
consume_each ( samples_offset ) ; // shift input to perform alignment with local replica
return 1 ;
}
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// Perform carrier wipe-off and compute Early, Prompt and Late correlation
2019-10-26 20:24:42 +00:00
multicorrelator_cpu . set_input_output_vectors ( d_correlator_outs . data ( ) , in ) ;
2018-04-10 08:52:19 +00:00
multicorrelator_cpu . Carrier_wipeoff_multicorrelator_resampler ( d_rem_carr_phase_rad ,
2020-07-10 17:52:28 +00:00
static_cast < float > ( d_carrier_phase_step_rad ) ,
static_cast < float > ( d_rem_code_phase_chips ) ,
static_cast < float > ( d_code_phase_step_chips ) ,
static_cast < float > ( d_code_phase_rate_step_chips ) ,
2018-04-10 08:52:19 +00:00
d_current_prn_length_samples ) ;
// ################## Kalman Carrier Tracking ######################################
// Kalman state prediction (time update)
2019-08-18 20:16:13 +00:00
kf_x_pre = kf_F * kf_x ; // state prediction
kf_P_x_pre = kf_F * kf_P_x * kf_F . t ( ) + kf_Q ; // state error covariance prediction
2018-04-10 08:52:19 +00:00
// Update discriminator [rads/Ti]
2018-08-14 01:18:01 +00:00
d_carr_phase_error_rad = pll_cloop_two_quadrant_atan ( d_correlator_outs [ 1 ] ) ; // prompt output
2018-04-10 08:52:19 +00:00
// Kalman estimation (measurement update)
double sigma2_phase_detector_cycles2 ;
2021-12-15 01:48:18 +00:00
const double CN_lin = pow ( 10 , d_CN0_SNV_dB_Hz / 10.0 ) ;
2019-08-31 09:37:29 +00:00
sigma2_phase_detector_cycles2 = ( 1.0 / ( 2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S ) ) * ( 1.0 + 1.0 / ( 2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S ) ) ;
2018-04-10 08:52:19 +00:00
2018-08-14 01:18:01 +00:00
kf_y ( 0 ) = d_carr_phase_error_rad ; // measurement vector
kf_R ( 0 , 0 ) = sigma2_phase_detector_cycles2 ;
2018-04-10 08:52:19 +00:00
2018-08-14 01:18:01 +00:00
if ( bayes_run & & ( kf_iter > = bayes_ptrans ) )
{
bayes_estimator . update_sequential ( kf_y ) ;
}
if ( bayes_run & & ( kf_iter > = ( bayes_ptrans + bayes_strans ) ) )
{
// TODO: Resolve segmentation fault
kf_P_y = bayes_estimator . get_Psi_est ( ) ;
kf_R_est = kf_P_y - kf_H * kf_P_x_pre * kf_H . t ( ) ;
}
else
{
2018-08-21 13:00:57 +00:00
kf_P_y = kf_H * kf_P_x_pre * kf_H . t ( ) + kf_R ; // innovation covariance matrix
2018-08-14 01:18:01 +00:00
kf_R_est = kf_R ;
}
2018-04-10 08:52:19 +00:00
2018-08-14 01:18:01 +00:00
// Kalman filter update step
2018-08-21 13:00:57 +00:00
kf_K = ( kf_P_x_pre * kf_H . t ( ) ) * arma : : inv ( kf_P_y ) ; // Kalman gain
kf_x = kf_x_pre + kf_K * kf_y ; // updated state estimation
2018-07-03 18:47:17 +00:00
kf_P_x = ( arma : : eye ( size ( kf_P_x_pre ) ) - kf_K * kf_H ) * kf_P_x_pre ; // update state estimation error covariance matrix
2018-04-10 08:52:19 +00:00
2018-08-14 01:15:58 +00:00
// Store Kalman filter results
2018-04-10 08:52:19 +00:00
d_rem_carr_phase_rad = kf_x ( 0 ) ; // set a new carrier Phase estimation to the NCO
d_carrier_doppler_hz = kf_x ( 1 ) ; // set a new carrier Doppler estimation to the NCO
2018-08-14 01:15:58 +00:00
if ( kf_x . n_elem > 2 )
{
d_carrier_dopplerrate_hz2 = kf_x ( 2 ) ;
}
else
{
d_carrier_dopplerrate_hz2 = 0 ;
}
2018-08-14 01:18:01 +00:00
d_carr_phase_sigma2 = kf_R_est ( 0 , 0 ) ;
2018-04-10 08:52:19 +00:00
// ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation
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 ) ;
2018-04-10 08:52:19 +00:00
// DLL discriminator
2020-07-10 17:52:28 +00:00
code_error_chips = dll_nc_e_minus_l_normalized ( d_correlator_outs [ 0 ] , d_correlator_outs [ 2 ] , static_cast < float > ( d_early_late_spc_chips ) , 1.0 ) ; // [chips/Ti] early and late
2018-04-10 08:52:19 +00:00
// Code discriminator filter
2020-07-10 17:52:28 +00:00
code_error_filt_chips = d_code_loop_filter . get_code_nco ( static_cast < float > ( code_error_chips ) ) ; // [chips/second]
2018-04-10 08:52:19 +00:00
double T_chip_seconds = 1.0 / static_cast < double > ( d_code_freq_chips ) ;
double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS ;
double code_error_filt_secs = ( T_prn_seconds * code_error_filt_chips * T_chip_seconds ) ; // [seconds]
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
double T_prn_samples = T_prn_seconds * static_cast < double > ( d_fs_in ) ;
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast < double > ( d_fs_in ) ;
d_current_prn_length_samples = static_cast < int > ( round ( K_blk_samples ) ) ; // round to a discrete number of samples
2022-11-24 08:12:33 +00:00
// ################### NCO COMMANDS #################################################
2018-04-10 08:52:19 +00:00
// carrier phase step (NCO phase increment per sample) [rads/sample]
2020-07-05 18:20:02 +00:00
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast < double > ( d_fs_in ) ;
2018-04-10 08:52:19 +00:00
// carrier phase accumulator
d_acc_carrier_phase_rad - = d_carrier_phase_step_rad * static_cast < double > ( d_current_prn_length_samples ) ;
2022-11-24 08:12:33 +00:00
// ################### DLL COMMANDS #################################################
2018-04-10 08:52:19 +00:00
// code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / static_cast < double > ( d_fs_in ) ;
// remnant code phase [chips]
d_rem_code_phase_samples = K_blk_samples - static_cast < double > ( d_current_prn_length_samples ) ; // rounding error < 1 sample
d_rem_code_phase_chips = d_code_freq_chips * ( d_rem_code_phase_samples / static_cast < double > ( d_fs_in ) ) ;
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if ( d_cn0_estimation_counter < FLAGS_cn0_samples )
{
// 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
2018-04-10 08:52:19 +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 ) ;
2018-04-10 08:52:19 +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 ) ;
2018-04-10 08:52:19 +00:00
// Loss of lock detection
if ( d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min )
{
2019-08-18 20:16:13 +00:00
// if (d_channel == 1)
2020-07-07 16:53:50 +00:00
// std::cout << "Carrier Lock Test Fail in channel " << d_channel << ": " << d_carrier_lock_test << " < " << d_carrier_lock_threshold << "," << nfail++ << '\n';
2018-04-10 08:52:19 +00:00
d_carrier_lock_fail_counter + + ;
2019-08-18 20:16:13 +00:00
// nfail++;
2018-04-10 08:52:19 +00:00
}
else
{
2019-02-11 20:13:02 +00:00
if ( d_carrier_lock_fail_counter > 0 )
{
d_carrier_lock_fail_counter - - ;
}
2018-04-10 08:52:19 +00:00
}
if ( d_carrier_lock_fail_counter > FLAGS_max_lock_fail )
{
2023-12-21 12:57:41 +00:00
gr : : thread : : scoped_lock l ( d_setlock ) ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Loss of lock in channel " < < d_channel < < " ! \n " ;
2018-04-10 08:52:19 +00:00
LOG ( INFO ) < < " Loss of lock in channel " < < d_channel < < " ! " ;
this - > message_port_pub ( pmt : : mp ( " events " ) , pmt : : from_long ( 3 ) ) ; // 3 -> loss of lock
d_carrier_lock_fail_counter = 0 ;
d_enable_tracking = false ;
2020-10-09 18:55:31 +00:00
loss_of_lock = true ;
2018-04-10 08:52:19 +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-21 13:00:57 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + static_cast < uint64_t > ( d_current_prn_length_samples ) ;
2018-04-10 08:52:19 +00:00
current_synchro_data . Code_phase_samples = d_rem_code_phase_samples ;
current_synchro_data . Carrier_phase_rads = d_acc_carrier_phase_rad ;
current_synchro_data . Carrier_Doppler_hz = d_carrier_doppler_hz ;
current_synchro_data . CN0_dB_hz = d_CN0_SNV_dB_Hz ;
2020-10-09 18:55:31 +00:00
current_synchro_data . Flag_valid_symbol_output = ! loss_of_lock ;
2018-04-10 08:52:19 +00:00
current_synchro_data . correlation_length_ms = 1 ;
2018-08-14 01:18:01 +00:00
kf_iter + + ;
2018-04-10 08:52:19 +00:00
}
else
{
2018-08-21 13:00:57 +00:00
for ( int32_t n = 0 ; n < d_n_correlator_taps ; n + + )
2018-04-10 08:52:19 +00:00
{
d_correlator_outs [ n ] = gr_complex ( 0 , 0 ) ;
}
2018-08-21 13:00:57 +00:00
current_synchro_data . Tracking_sample_counter = d_sample_counter + static_cast < uint64_t > ( d_current_prn_length_samples ) ;
2018-04-10 08:52:19 +00:00
current_synchro_data . System = { ' G ' } ;
current_synchro_data . correlation_length_ms = 1 ;
}
// assign the GNU Radio block output data
current_synchro_data . fs = d_fs_in ;
2023-11-27 12:41:55 +00:00
* out [ 0 ] = std : : move ( current_synchro_data ) ;
2018-04-10 08:52:19 +00:00
if ( d_dump )
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I ;
float prompt_Q ;
2019-08-12 22:19:31 +00:00
float tmp_E ;
float tmp_P ;
float tmp_L ;
2018-04-10 08:52:19 +00:00
float tmp_VE = 0.0 ;
float tmp_VL = 0.0 ;
float tmp_float ;
double tmp_double ;
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 ] ) ;
try
{
// EPR
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VE ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_E ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_P ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_L ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_VL ) , sizeof ( float ) ) ;
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_I ) , sizeof ( float ) ) ;
d_dump_file . write ( reinterpret_cast < char * > ( & prompt_Q ) , sizeof ( float ) ) ;
// PRN start sample stamp
2018-08-21 13:00:57 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & d_sample_counter ) , sizeof ( uint64_t ) ) ;
2018-04-10 08:52:19 +00:00
// accumulated carrier phase
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_acc_carrier_phase_rad ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// carrier and code frequency
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_carrier_doppler_hz ) ;
2018-04-10 08:52:19 +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_dopplerrate_hz2 ) ;
2018-08-14 01:15:58 +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-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-08-14 01:15:58 +00:00
// Kalman commands
2020-07-05 18:20:02 +00:00
tmp_float = static_cast < float > ( d_carr_phase_error_rad * TWO_PI ) ;
2018-08-14 01:18:01 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
tmp_float = static_cast < float > ( d_carr_phase_sigma2 ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2020-07-05 18:20:02 +00:00
tmp_float = static_cast < float > ( d_rem_carr_phase_rad * TWO_PI ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// DLL commands
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( code_error_chips ) ;
2018-04-10 08:52:19 +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 > ( code_error_filt_chips ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// CN0 and carrier lock test
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_CN0_SNV_dB_Hz ) ;
2018-04-10 08:52:19 +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-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
// AUX vars (for debug purposes)
2020-07-10 17:52:28 +00:00
tmp_float = static_cast < float > ( d_rem_code_phase_samples ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_float ) , sizeof ( float ) ) ;
2018-08-21 13:00:57 +00:00
tmp_double = static_cast < double > ( d_sample_counter + static_cast < uint64_t > ( d_current_prn_length_samples ) ) ;
2018-04-10 08:52:19 +00:00
d_dump_file . write ( reinterpret_cast < char * > ( & tmp_double ) , sizeof ( double ) ) ;
// PRN
2018-08-21 13:00:57 +00:00
uint32_t prn_ = d_acquisition_gnss_synchro - > PRN ;
d_dump_file . write ( reinterpret_cast < char * > ( & prn_ ) , sizeof ( uint32_t ) ) ;
2018-04-10 08:52:19 +00:00
}
2022-12-11 11:48:00 +00:00
catch ( const std : : ofstream : : failure & e )
2018-04-10 08:52:19 +00:00
{
LOG ( WARNING ) < < " Exception writing trk dump file " < < e . what ( ) ;
}
}
consume_each ( d_current_prn_length_samples ) ; // this is necessary in gr::block derivates
d_sample_counter + = d_current_prn_length_samples ; // count for the processed samples
return 1 ; // output tracking result ALWAYS even in the case of d_enable_tracking==false
}