From 559080b651301b7be3e9073988ab0dc8038bb7f9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 11 Jun 2019 20:20:23 +0200 Subject: [PATCH 1/2] Improving tracking stability of carrier lock detector --- src/algorithms/libs/gnss_sdr_flags.cc | 7 +- src/algorithms/libs/gnss_sdr_flags.h | 13 +- .../gps_l1_ca_telemetry_decoder_gs.cc | 9 +- .../gps_l5_telemetry_decoder_gs.cc | 2 +- .../adapters/beidou_b1i_dll_pll_tracking.cc | 30 +---- .../adapters/beidou_b3i_dll_pll_tracking.cc | 17 +-- .../galileo_e1_dll_pll_veml_tracking.cc | 29 +--- .../adapters/galileo_e5a_dll_pll_tracking.cc | 29 +--- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 29 +--- .../adapters/gps_l2_m_dll_pll_tracking.cc | 29 +--- .../adapters/gps_l5_dll_pll_tracking.cc | 30 +---- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 127 +++++++++++------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 3 +- src/algorithms/tracking/libs/CMakeLists.txt | 1 + src/algorithms/tracking/libs/dll_pll_conf.cc | 12 +- src/algorithms/tracking/libs/dll_pll_conf.h | 3 +- 16 files changed, 151 insertions(+), 219 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_flags.cc b/src/algorithms/libs/gnss_sdr_flags.cc index 756d428f4..ccf30fc55 100644 --- a/src/algorithms/libs/gnss_sdr_flags.cc +++ b/src/algorithms/libs/gnss_sdr_flags.cc @@ -65,9 +65,12 @@ DEFINE_int32(cn0_samples, 20, "Number of correlator outputs used for CN0 estimat DEFINE_int32(cn0_min, 25, "Minimum valid CN0 (in dB-Hz)."); -DEFINE_int32(max_lock_fail, 50, "Maximum number of lock failures before dropping a satellite."); +DEFINE_int32(max_carrier_lock_fail, 5000, "Maximum number of carrier lock failures before dropping a satellite."); -DEFINE_double(carrier_lock_th, 0.85, "Carrier lock threshold (in rad)."); +DEFINE_int32(max_lock_fail, 50, "Maximum number of code lock failures before dropping a satellite."); + +//cos(2xError_angle)=0.7 -> Error_angle=22 deg +DEFINE_double(carrier_lock_th, 0.7, "Carrier lock threshold (in rad)."); DEFINE_string(RINEX_version, "-", "If defined, specifies the RINEX version (2.11 or 3.02). Overrides the configuration file."); diff --git a/src/algorithms/libs/gnss_sdr_flags.h b/src/algorithms/libs/gnss_sdr_flags.h index c07edc563..28bb79334 100644 --- a/src/algorithms/libs/gnss_sdr_flags.h +++ b/src/algorithms/libs/gnss_sdr_flags.h @@ -48,12 +48,13 @@ DECLARE_int32(doppler_max); //!< If defined, maximum Doppler value in the sear DECLARE_int32(doppler_step); //!< If defined, sets the frequency step in the search grid, in Hz, in Hz (overrides the configuration file). // Declare flags for tracking blocks -DECLARE_int32(cn0_samples); //!< Number of correlator outputs used for CN0 estimation. -DECLARE_int32(cn0_min); //!< Minimum valid CN0 (in dB-Hz). -DECLARE_int32(max_lock_fail); //!< Maximum number of lock failures before dropping a satellite. -DECLARE_double(carrier_lock_th); //!< Carrier lock threshold (in rad). -DECLARE_double(dll_bw_hz); //!< Bandwidth of the DLL low pass filter, in Hz (overrides the configuration file). -DECLARE_double(pll_bw_hz); //!< Bandwidth of the PLL low pass filter, in Hz (overrides the configuration file). +DECLARE_int32(cn0_samples); //!< Number of correlator outputs used for CN0 estimation. +DECLARE_int32(cn0_min); //!< Minimum valid CN0 (in dB-Hz). +DECLARE_int32(max_lock_fail); //!< Maximum number of code lock failures before dropping a satellite. +DECLARE_int32(max_carrier_lock_fail); //!< Maximum number of carrier lock failures before dropping a satellite. +DECLARE_double(carrier_lock_th); //!< Carrier lock threshold (in rad). +DECLARE_double(dll_bw_hz); //!< Bandwidth of the DLL low pass filter, in Hz (overrides the configuration file). +DECLARE_double(pll_bw_hz); //!< Bandwidth of the PLL low pass filter, in Hz (overrides the configuration file). // Declare flags for PVT DECLARE_string(RINEX_version); //!< If defined, specifies the RINEX version (2.11 or 3.02). Overrides the configuration file. diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 7d06833e2..eb5d43f4f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -430,7 +430,14 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp - if (corr_value < 0) flag_PLL_180_deg_phase_locked = true; + if (corr_value < 0) + { + flag_PLL_180_deg_phase_locked = true; + } + else + { + flag_PLL_180_deg_phase_locked = false; + } d_stat = 2; } else diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index 660b0df4d..21f786819 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -64,7 +64,7 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry_to_trk")); d_last_valid_preamble = 0; d_sent_tlm_failed_msg = false; - d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 5; //rise alarm if 5 consecutive subframes have no valid CRC + d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 10 consecutive subframes have no valid CRC // initialize internal vars d_dump = dump; diff --git a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc index 8bfcbe82f..f85a80e23 100644 --- a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc @@ -152,30 +152,12 @@ BeidouB1iDllPllTracking::BeidouB1iDllPllTracking( trk_param.system = 'C'; char sig_[3] = "B1"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index 49d76417c..2b0993c47 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -147,18 +147,11 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.system = 'C'; char sig_[3] = "B3"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; - trk_param.carrier_lock_th = carrier_lock_th; + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index 174112c03..ced7ccda8 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -162,30 +162,11 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( trk_param.system = 'E'; char sig_[3] = "1B"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index cdf4d7cad..9845f2c14 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -159,30 +159,11 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( trk_param.system = 'E'; char sig_[3] = "5X"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index a511503fb..608223fc9 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -165,30 +165,11 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( trk_param.system = 'G'; char sig_[3] = "1C"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 30); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.80); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 65912ccbf..e36176705 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -141,30 +141,11 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( trk_param.system = 'G'; char sig_[3] = "2S"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc index 988f8dccb..bf06c34a9 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -160,30 +160,12 @@ GpsL5DllPllTracking::GpsL5DllPllTracking( trk_param.system = 'G'; char sig_[3] = "L5"; std::memcpy(trk_param.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param.carrier_lock_th = carrier_lock_th; + + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); //################# MAKE TRACKING GNURadio object ################### if (item_type == "gr_complex") diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2e9610bf5..d411f2e45 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -444,6 +444,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; d_carrier_lock_threshold = trk_parameters.carrier_lock_th; d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_cn0_smoother = Exponential_Smoother(); @@ -451,6 +452,13 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl { d_cn0_smoother.set_samples_for_initialization(200 / static_cast(d_code_period * 1000.0)); } + + 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); + d_acquisition_gnss_synchro = nullptr; d_channel = 0; d_acq_code_phase_samples = 0.0; @@ -717,6 +725,7 @@ void dll_pll_veml_tracking::start_tracking() std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; d_rem_code_phase_samples = 0.0; d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; @@ -866,11 +875,13 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer, 1)); + //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection if (!d_pull_in_transitory) { - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + //d_carrier_lock_test < d_carrier_lock_threshold or + if (d_carrier_lock_test < d_carrier_lock_threshold) { d_carrier_lock_fail_counter++; } @@ -881,13 +892,28 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ d_carrier_lock_fail_counter--; } } + + 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--; + } + } } - if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) + if (d_carrier_lock_fail_counter > trk_parameters.max_carrier_lock_fail or d_code_lock_fail_counter > trk_parameters.max_code_lock_fail) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + LOG(INFO) << "Loss of lock in channel " << d_channel + << " (carrier_lock_fail_counter:" << d_carrier_lock_fail_counter + << " code_lock_fail_counter : " << d_code_lock_fail_counter << ")"; this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; return false; } return true; @@ -1573,6 +1599,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) { d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; } } switch (d_state) @@ -1606,6 +1634,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_state = 2; d_sample_counter += samples_offset; // count for the processed samples d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz @@ -1642,51 +1671,59 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // enable write dump file this cycle (valid DLL/PLL cycle) log_data(false); - if (d_secondary) + + if (!d_pull_in_transitory) { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_circular_buffer.push_back(*d_Prompt); - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + if (d_secondary) { - next_state = acquire_secondary(); - if (next_state) + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) { - 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; - } - } - } - else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change - { - float current_tracking_time_s = static_cast(d_sample_counter - d_acq_sample_stamp) / trk_parameters.fs_in; - if (current_tracking_time_s > 10) - { - d_symbol_history.push_back(d_Prompt->real()); - //******* preamble correlation ******** - int32_t corr_value = 0; - if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) - { - int i = 0; - for (const auto &iter : d_symbol_history) + next_state = acquire_secondary(); + if (next_state) { - if (iter < 0.0) // symbols clipping - { - corr_value -= d_preambles_symbols[i]; - } - else - { - corr_value += d_preambles_symbols[i]; - } - i++; + 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; } } - if (corr_value == d_preamble_length_symbols) + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { + float current_tracking_time_s = static_cast(d_sample_counter - d_acq_sample_stamp) / trk_parameters.fs_in; + if (current_tracking_time_s > 10) { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - next_state = true; + d_symbol_history.push_back(d_Prompt->real()); + //******* preamble correlation ******** + int32_t corr_value = 0; + if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) + { + int i = 0; + for (const auto &iter : d_symbol_history) + { + if (iter < 0.0) // symbols clipping + { + corr_value -= d_preambles_symbols[i]; + } + else + { + corr_value += d_preambles_symbols[i]; + } + i++; + } + } + if (corr_value == d_preamble_length_symbols) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + next_state = true; + } + else + { + next_state = false; + } } else { @@ -1695,14 +1732,13 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else { - next_state = false; + next_state = true; } } else { - next_state = true; + next_state = false; //keep in state 2 during pull-in transitory } - // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) { @@ -1837,7 +1873,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_extend_correlation_symbols_count = 0; d_state = 4; } - log_data(true); break; } case 4: // narrow tracking diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 1ccb44667..d85058589 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -193,13 +193,14 @@ private: // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; int32_t d_carrier_lock_fail_counter; + int32_t d_code_lock_fail_counter; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; gr_complex *d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; - + Exponential_Smoother d_carrier_lock_test_smoother; // file dump std::ofstream d_dump_file; std::string d_dump_filename; diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 3f772f149..111c590ec 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -82,6 +82,7 @@ target_link_libraries(tracking_libs Gnuradio::runtime Volkgnsssdr::volkgnsssdr core_system_parameters + algorithms_libs ${OPT_TRACKING_LIBRARIES} PRIVATE Gflags::gflags diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index a2e224d7d..fad3277aa 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -31,6 +31,7 @@ #include "dll_pll_conf.h" +#include "gnss_sdr_flags.h" #include Dll_Pll_Conf::Dll_Pll_Conf() @@ -61,11 +62,12 @@ Dll_Pll_Conf::Dll_Pll_Conf() early_late_space_narrow_chips = 0.1; very_early_late_space_narrow_chips = 0.1; extend_correlation_symbols = 5; - cn0_samples = 20; - carrier_lock_det_mav_samples = 20; - cn0_min = 25; - max_lock_fail = 50; - carrier_lock_th = 0.85; + cn0_samples = FLAGS_cn0_samples; + carrier_lock_det_mav_samples = FLAGS_cn0_samples; + cn0_min = FLAGS_cn0_min; + max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; + max_code_lock_fail = FLAGS_max_lock_fail; + carrier_lock_th = FLAGS_carrier_lock_th; track_pilot = false; system = 'G'; char sig_[3] = "1C"; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 4a04c02e9..94ac5e270 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -69,7 +69,8 @@ public: int32_t cn0_samples; int32_t carrier_lock_det_mav_samples; int32_t cn0_min; - int32_t max_lock_fail; + int32_t max_code_lock_fail; + int32_t max_carrier_lock_fail; uint32_t smoother_length; double carrier_lock_th; bool track_pilot; From 88f78a107c1d95507c34c813db732a0459fe0231 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 14 Jun 2019 12:52:46 +0200 Subject: [PATCH 2/2] Improving unified tracking. Added Doppler frequency correction for false PLL lock --- src/algorithms/libs/gnss_sdr_flags.cc | 2 +- .../galileo_telemetry_decoder_gs.cc | 9 ++++--- .../gps_l5_telemetry_decoder_gs.cc | 2 +- .../adapters/beidou_b1i_dll_pll_tracking.cc | 3 +-- .../adapters/beidou_b3i_dll_pll_tracking.cc | 3 +-- .../galileo_e1_dll_pll_veml_tracking.cc | 3 +-- .../adapters/galileo_e5a_dll_pll_tracking.cc | 5 ++-- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 3 +-- .../adapters/gps_l2_m_dll_pll_tracking.cc | 3 +-- .../adapters/gps_l5_dll_pll_tracking.cc | 5 ++-- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 25 ++++++++++++++++--- .../gnuradio_blocks/dll_pll_veml_tracking.h | 3 +++ src/algorithms/tracking/libs/dll_pll_conf.cc | 2 +- 13 files changed, 43 insertions(+), 25 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_flags.cc b/src/algorithms/libs/gnss_sdr_flags.cc index 6e021b73c..871776a52 100644 --- a/src/algorithms/libs/gnss_sdr_flags.cc +++ b/src/algorithms/libs/gnss_sdr_flags.cc @@ -67,7 +67,7 @@ DEFINE_int32(cn0_samples, 20, "Number of correlator outputs used for CN0 estimat DEFINE_int32(cn0_min, 25, "Minimum valid CN0 (in dB-Hz)."); -DEFINE_int32(max_carrier_lock_fail, 5000, "Maximum number of carrier lock failures before dropping a satellite."); +DEFINE_int32(max_carrier_lock_fail, 10000, "Maximum number of carrier lock failures before dropping a satellite."); DEFINE_int32(max_lock_fail, 50, "Maximum number of code lock failures before dropping a satellite."); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 00b20a8fd..a94aebcf8 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -97,7 +97,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - d_max_symbols_without_valid_frame = GALILEO_INAV_PAGE_PART_SYMBOLS * 30; //rise alarm 30 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_INAV_PAGE_SYMBOLS * 30; //rise alarm 60 seconds without valid tlm break; } @@ -127,7 +127,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_secondary_code_samples[i] = -1; } } - d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_FNAV_SYMBOLS_PER_PAGE * 30; //rise alarm 30 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm break; } default: @@ -452,8 +452,10 @@ void galileo_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satellite void galileo_telemetry_decoder_gs::reset() { + gr::thread::scoped_lock lock(d_setlock); d_last_valid_preamble = d_sample_counter; d_sent_tlm_failed_msg = false; + d_stat = 0; DLOG(INFO) << "Telemetry decoder reset for satellite " << d_satellite; } @@ -501,11 +503,12 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_flag_preamble = false; // check if there is a problem with the telemetry of the current satellite - if (d_stat < 1 and d_sent_tlm_failed_msg == false) + if (d_sent_tlm_failed_msg == false) { if ((d_sample_counter - d_last_valid_preamble) > d_max_symbols_without_valid_frame) { int message = 1; //bad telemetry + DLOG(INFO) << "sent msg sat " << this->d_satellite; this->message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(message)); d_sent_tlm_failed_msg = true; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index 21f786819..6b8ef3086 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -64,7 +64,7 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry_to_trk")); d_last_valid_preamble = 0; d_sent_tlm_failed_msg = false; - d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 10 consecutive subframes have no valid CRC + d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 20; //rise alarm if 20 consecutive subframes have no valid CRC // initialize internal vars d_dump = dump; diff --git a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc index f85a80e23..758f4b472 100644 --- a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc @@ -113,8 +113,7 @@ BeidouB1iDllPllTracking::BeidouB1iDllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index 2b0993c47..306a0efb5 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -108,8 +108,7 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index ced7ccda8..6bcff1454 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -128,8 +128,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index 9845f2c14..bf0587746 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -123,8 +123,7 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; @@ -135,7 +134,7 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( int vector_length = std::round(fs_in / (GALILEO_E5A_CODE_CHIP_RATE_HZ / GALILEO_E5A_CODE_LENGTH_CHIPS)); trk_param.vector_length = vector_length; int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); - float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; bool track_pilot = configuration->property(role + ".track_pilot", false); if (extend_correlation_symbols < 1) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 608223fc9..0202a836b 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -129,8 +129,7 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index e36176705..cf65d521a 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -117,8 +117,7 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); trk_param.vector_length = vector_length; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc index bf06c34a9..98b7db3ec 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -130,13 +130,12 @@ GpsL5DllPllTracking::GpsL5DllPllTracking( trk_param.enable_fll_pull_in = enable_fll_pull_in; float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); trk_param.fll_bw_hz = fll_bw_hz; - float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); - trk_param.pull_in_time_s = pull_in_time_s; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5I_CODE_RATE_HZ) / static_cast(GPS_L5I_CODE_LENGTH_CHIPS))); trk_param.vector_length = vector_length; int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); - float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; bool track_pilot = configuration->property(role + ".track_pilot", false); if (extend_correlation_symbols < 1) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index f279825aa..7864e747e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -99,6 +99,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars + d_dll_filt_history.set_capacity(2000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; @@ -525,6 +526,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_dump = false; } } + d_corrected_doppler = false; } @@ -553,7 +555,7 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); - d_carrier_lock_fail_counter = 10000; //force loss-of-lock condition + d_carrier_lock_fail_counter = 100000; //force loss-of-lock condition break; } default: @@ -771,6 +773,7 @@ void dll_pll_veml_tracking::start_tracking() d_cloop = true; d_pull_in_transitory = true; d_Prompt_circular_buffer.clear(); + d_corrected_doppler = false; } @@ -1016,9 +1019,25 @@ void dll_pll_veml_tracking::run_dll_pll() } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] - // 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; + + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler + if (d_pull_in_transitory == false and d_corrected_doppler == false) + { + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + if (d_dll_filt_history.full()) + { + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(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; + } + } + } } @@ -1641,7 +1660,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_cn0_smoother.reset(); d_carrier_lock_test_smoother.reset(); - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index d85058589..38969900a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -113,6 +113,8 @@ private: int32_t d_preamble_length_symbols; boost::circular_buffer d_symbol_history; + // dll filter buffer + boost::circular_buffer d_dll_filt_history; // tracking state machine int32_t d_state; @@ -170,6 +172,7 @@ private: // tracking vars bool d_pull_in_transitory; + bool d_corrected_doppler; double d_current_correlation_time_s; double d_carr_phase_error_hz; double d_carr_freq_error_hz; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index fad3277aa..8bb114e96 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -46,7 +46,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() dump_filename = std::string("./dll_pll_dump.dat"); enable_fll_pull_in = false; enable_fll_steady_state = false; - pull_in_time_s = 2; + pull_in_time_s = 10; fll_filter_order = 1; pll_filter_order = 3; dll_filter_order = 2;