diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index e22c6f509..c034b251c 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -25,7 +25,7 @@ if(ENABLE_FPGA) SET(OPT_TRACKING_ADAPTERS ${OPT_TRACKING_ADAPTERS} gps_l1_ca_dll_pll_c_aid_tracking_fpga.cc) endif(ENABLE_FPGA) -set(TRACKING_ADAPTER_SOURCES +set(TRACKING_ADAPTER_SOURCES galileo_e1_dll_pll_veml_tracking.cc galileo_e1_tcp_connector_tracking.cc gps_l1_ca_dll_pll_tracking.cc @@ -40,7 +40,7 @@ set(TRACKING_ADAPTER_SOURCES glonass_l2_ca_dll_pll_c_aid_tracking.cc ${OPT_TRACKING_ADAPTERS} ) - + include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_SOURCE_DIR}/src/core/system_parameters 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 437f860ce..f6ca8df87 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 @@ -63,7 +63,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; - unified_ = configuration->property(role + ".unified", false); item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); @@ -92,44 +91,23 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - if (unified_) - { - char sig_[3] = "1B"; - tracking_unified_ = dll_pll_veml_make_tracking( - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot, 'E', sig_); - } - else - { - tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( - 0, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot); - } + + char sig_[3] = "1B"; + tracking_ = dll_pll_veml_make_tracking( + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'E', sig_); } else { @@ -150,10 +128,7 @@ GalileoE1DllPllVemlTracking::~GalileoE1DllPllVemlTracking() void GalileoE1DllPllVemlTracking::start_tracking() { - if (unified_) - tracking_unified_->start_tracking(); - else - tracking_->start_tracking(); + tracking_->start_tracking(); } @@ -163,19 +138,13 @@ void GalileoE1DllPllVemlTracking::start_tracking() void GalileoE1DllPllVemlTracking::set_channel(unsigned int channel) { channel_ = channel; - if (unified_) - tracking_unified_->set_channel(channel); - else - tracking_->set_channel(channel); + tracking_->set_channel(channel); } void GalileoE1DllPllVemlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - if (unified_) - tracking_unified_->set_gnss_synchro(p_gnss_synchro); - else - tracking_->set_gnss_synchro(p_gnss_synchro); + tracking_->set_gnss_synchro(p_gnss_synchro); } @@ -199,17 +168,11 @@ void GalileoE1DllPllVemlTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_left_block() { - if (unified_) - return tracking_unified_; - else - return tracking_; + return tracking_; } gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_right_block() { - if (unified_) - return tracking_unified_; - else - return tracking_; + return tracking_; } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h index 2d3dfd221..ab72a2efb 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h @@ -38,7 +38,6 @@ #define GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_H_ #include "tracking_interface.h" -#include "galileo_e1_dll_pll_veml_tracking_cc.h" #include "dll_pll_veml_tracking.h" #include @@ -95,14 +94,12 @@ public: void start_tracking() override; private: - galileo_e1_dll_pll_veml_tracking_cc_sptr tracking_; - dll_pll_veml_tracking_sptr tracking_unified_; + dll_pll_veml_tracking_sptr tracking_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - bool unified_; }; #endif // GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_H_ 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 ec5dbcf98..b95b003f2 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 @@ -60,7 +60,6 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); dump = configuration->property(role + ".dump", false); - unified_ = configuration->property(role + ".unified", false); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); @@ -78,38 +77,23 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - if (unified_) - { - char sig_[3] = "1C"; - tracking_unified_ = dll_pll_veml_make_tracking( - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - early_late_space_chips, - early_late_space_narrow_chips, - early_late_space_narrow_chips, - symbols_extended_correlator, - false, - 'G', sig_); - } - else - { - tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( - 0, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); - } + char sig_[3] = "1C"; + tracking_ = dll_pll_veml_make_tracking( + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + symbols_extended_correlator, + false, + 'G', sig_); } else { @@ -128,10 +112,7 @@ GpsL1CaDllPllTracking::~GpsL1CaDllPllTracking() void GpsL1CaDllPllTracking::start_tracking() { - if (unified_) - tracking_unified_->start_tracking(); - else - tracking_->start_tracking(); + tracking_->start_tracking(); } @@ -141,19 +122,13 @@ void GpsL1CaDllPllTracking::start_tracking() void GpsL1CaDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - if (unified_) - tracking_unified_->set_channel(channel); - else - tracking_->set_channel(channel); + tracking_->set_channel(channel); } void GpsL1CaDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - if (unified_) - tracking_unified_->set_gnss_synchro(p_gnss_synchro); - else - tracking_->set_gnss_synchro(p_gnss_synchro); + tracking_->set_gnss_synchro(p_gnss_synchro); } @@ -177,17 +152,11 @@ void GpsL1CaDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL1CaDllPllTracking::get_left_block() { - if (unified_) - return tracking_unified_; - else - return tracking_; + return tracking_; } gr::basic_block_sptr GpsL1CaDllPllTracking::get_right_block() { - if (unified_) - return tracking_unified_; - else - return tracking_; + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h index 5bf34abfc..2b5557917 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h @@ -39,7 +39,6 @@ #define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_H_ #include "tracking_interface.h" -#include "gps_l1_ca_dll_pll_tracking_cc.h" #include "dll_pll_veml_tracking.h" #include @@ -93,14 +92,12 @@ public: void start_tracking() override; private: - gps_l1_ca_dll_pll_tracking_cc_sptr tracking_; - dll_pll_veml_tracking_sptr tracking_unified_; + dll_pll_veml_tracking_sptr tracking_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - bool unified_; }; #endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 759b774a1..3c3fb03d7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -27,9 +27,7 @@ if(ENABLE_FPGA) endif(ENABLE_FPGA) set(TRACKING_GR_BLOCKS_SOURCES - galileo_e1_dll_pll_veml_tracking_cc.cc galileo_e1_tcp_connector_tracking_cc.cc - gps_l1_ca_dll_pll_tracking_cc.cc gps_l1_ca_tcp_connector_tracking_cc.cc galileo_e5a_dll_pll_tracking_cc.cc gps_l2_m_dll_pll_tracking_cc.cc 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 88350e361..e32905196 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -193,6 +193,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( { LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; } } else if (system == 'E') @@ -246,6 +254,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( { LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; } } else @@ -901,12 +917,15 @@ void dll_pll_veml_tracking::log_data(bool integrating) if (integrating) { // It compensates the amplitude difference while integrating - float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; + if (d_extend_correlation_symbols_count > 0) + { + float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); + tmp_VE *= scale_factor; + tmp_E *= scale_factor; + tmp_P *= scale_factor; + tmp_L *= scale_factor; + tmp_VL *= scale_factor; + } } try diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc deleted file mode 100644 index 4cfbe9305..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ /dev/null @@ -1,1236 +0,0 @@ -/*! - * \file galileo_e1_dll_pll_veml_tracking_cc.cc - * \brief Implementation of a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals - * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com - * - * Code DLL + carrier PLL according to the algorithms described in: - * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkhauser, 2007 - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "galileo_e1_dll_pll_veml_tracking_cc.h" -#include "galileo_e1_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "Galileo_E1.h" -#include "control_message_factory.h" -#include "gnss_sdr_flags.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using google::LogMessage; - -galileo_e1_dll_pll_veml_tracking_cc_sptr -galileo_e1_dll_pll_veml_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot) -{ - return galileo_e1_dll_pll_veml_tracking_cc_sptr(new galileo_e1_dll_pll_veml_tracking_cc(if_freq, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot)); -} - - -void galileo_e1_dll_pll_veml_tracking_cc::forecast(int noutput_items, - gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } -} - - -galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot) : gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) -{ - // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); - this->set_relative_rate(1.0 / vector_length); - - this->message_port_register_out(pmt::mp("events")); - - // initialize internal vars - d_dump = dump; - d_if_freq = if_freq; - d_fs_in = fs_in; - d_vector_length = vector_length; - d_dump_filename = dump_filename; - d_code_loop_filter = Tracking_2nd_DLL_filter(Galileo_E1_CODE_PERIOD); - d_carrier_loop_filter = Tracking_2nd_PLL_filter(Galileo_E1_CODE_PERIOD); - - // Initialize tracking ========================================== - - // Set bandwidth of code and carrier loop filters - d_dll_bw_hz = dll_bw_hz; - d_pll_bw_hz = pll_bw_hz; - d_dll_bw_narrow_hz = dll_bw_narrow_hz; - d_pll_bw_narrow_hz = pll_bw_narrow_hz; - - d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); - - // Correlator spacing - d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) - d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips) - d_early_late_spc_narrow_chips = early_late_space_narrow_chips; // Define narrow early-late offset (in chips) - d_very_early_late_spc_narrow_chips = very_early_late_space_narrow_chips; // Define narrow very-early-late offset (in chips) - - // Initialization of local code replica - // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip - d_tracking_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - - // correlator outputs (scalar) - d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - // map memory pointers of correlator outputs - d_Very_Early = &d_correlator_outs[0]; - d_Early = &d_correlator_outs[1]; - d_Prompt = &d_correlator_outs[2]; - d_Late = &d_correlator_outs[3]; - d_Very_Late = &d_correlator_outs[4]; - - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_very_early_late_spc_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_chips; - - d_correlation_length_samples = d_vector_length; - multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); - - d_extend_correlation_symbols = extend_correlation_symbols; - // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal - d_track_pilot = track_pilot; - if (d_track_pilot) - { - // extended integration control - if (d_extend_correlation_symbols > 1) - { - d_enable_extended_integration = true; - } - else - { - d_enable_extended_integration = false; - } - // Extra correlator for the data component - d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); - d_local_code_data_shift_chips[0] = 0.0; - correlator_data_cpu.init(2 * d_correlation_length_samples, 1); - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_Prompt_Data[0] = gr_complex(0, 0); - d_data_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - } - else - { - // Disable extended integration if data component tracking is selected - d_enable_extended_integration = false; - d_local_code_data_shift_chips = nullptr; - d_data_code = nullptr; - d_Prompt_Data = nullptr; - } - - //--- Initializations ------------------------------ - // Initial code frequency basis of NCO - d_code_freq_chips = static_cast(Galileo_E1_CODE_CHIP_RATE_HZ); - // Residual code phase (in chips) - d_rem_code_phase_samples = 0.0; - // Residual carrier phase - d_rem_carr_phase_rad = 0.0; - - // sample synchronization - d_sample_counter = 0; - //d_sample_counter_seconds = 0; - d_acq_sample_stamp = 0; - - d_current_prn_length_samples = static_cast(d_vector_length); - - // CN0 estimation and lock detector buffers - d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = FLAGS_carrier_lock_th; - - systemName["E"] = std::string("Galileo"); - - clear_tracking_vars(); - - d_acquisition_gnss_synchro = 0; - d_channel = 0; - d_acq_code_phase_samples = 0.0; - d_acq_carrier_doppler_hz = 0.0; - d_carrier_doppler_hz = 0.0; - d_acc_carrier_phase_rad = 0.0; - - d_extend_correlation_symbols_count = 0; - d_code_phase_step_chips = 0.0; - d_carrier_phase_step_rad = 0.0; - d_rem_code_phase_chips = 0.0; - d_K_blk_samples = 0.0; - d_code_phase_samples = 0.0; - - d_state = 0; // initial state: standby -} - - -void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() -{ - /* - * correct the code phase according to the delay between acq and trk - */ - d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; - d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; - d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - - long int acq_trk_diff_samples; - double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - // Doppler effect - // Fd=(C/(C+Vr))*F - double radial_velocity = (Galileo_E1_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E1_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; - d_code_freq_chips = radial_velocity * Galileo_E1_CODE_CHIP_RATE_HZ; - d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1 / d_code_freq_chips; - T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); - - d_current_prn_length_samples = round(T_prn_mod_samples); - - double T_prn_true_seconds = Galileo_E1_B_CODE_LENGTH_CHIPS / Galileo_E1_CODE_CHIP_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(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; - double corrected_acq_phase_samples, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(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; - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - - // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter - - if (d_track_pilot) - { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, - pilot_signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - galileo_e1_code_gen_float_sampled(d_data_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - d_Prompt_Data[0] = gr_complex(0, 0); // clean data correlator output - correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), - d_data_code, - d_local_code_shift_chips); - } - else - { - galileo_e1_code_gen_float_sampled(d_tracking_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - } - - multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - 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; - - d_code_phase_samples = d_acq_code_phase_samples; - - std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); - - // DEBUG OUTPUT - std::cout << "Tracking of Galileo E1 signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking pull-in - d_state = 1; - - LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " Code Phase correction [samples]=" << delay_correction_samples - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; -} - - -galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() -{ - if (d_dump_file.is_open()) - { - try - { - d_dump_file.close(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } - } - if (d_dump) - { - if (d_channel == 0) - { - std::cout << "Writing .mat files ..."; - } - galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); - if (d_channel == 0) - { - std::cout << " done." << std::endl; - } - } - try - { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - volk_gnsssdr_free(d_tracking_code); - if (d_track_pilot) - { - volk_gnsssdr_free(d_Prompt_Data); - volk_gnsssdr_free(d_data_code); - volk_gnsssdr_free(d_local_code_data_shift_chips); - correlator_data_cpu.free(); - } - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -bool galileo_e1_dll_pll_veml_tracking_cc::acquire_secondary() -{ - //******* preamble correlation ******** - int corr_value = 0; - for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) - { - if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping - { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value++; - } - else - { - corr_value--; - } - } - else - { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value--; - } - else - { - corr_value++; - } - } - } - - if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - return true; - } - else - { - return false; - } -} - - -bool galileo_e1_dll_pll_veml_tracking_cc::cn0_and_tracking_lock_status() -{ - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < FLAGS_cn0_samples) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; - d_cn0_estimation_counter++; - return true; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - return false; - } - else - { - return true; - } - } -} - - -// correlation requires: -// - updated remnant carrier phase in radians (rem_carr_phase_rad) -// - updated remnant code phase in samples (d_rem_code_phase_samples) -// - d_code_freq_chips -// - d_carrier_doppler_hz -void galileo_e1_dll_pll_veml_tracking_cc::do_correlation_step(const gr_complex *input_samples) -{ - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); - - // DATA CORRELATOR (if tracking tracks the pilot signal) - if (d_track_pilot) - { - correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); - correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); - } -} - - -void galileo_e1_dll_pll_veml_tracking_cc::run_dll_pll(bool disable_costas_loop) -{ - // ################## PLL ########################################################## - // PLL discriminator - if (disable_costas_loop == true) - { - // Secondary code acquired. No symbols transition should be present in the signal - d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - } - else - { - // Costas loop discriminator, insensitive to 180 deg phase transitions - d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - } - - // Carrier discriminator filter - d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] - // Code discriminator filter - d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] -} - - -void galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() -{ - *d_Very_Early = gr_complex(0, 0); - *d_Early = gr_complex(0, 0); - *d_Prompt = gr_complex(0, 0); - *d_Late = gr_complex(0, 0); - *d_Very_Late = gr_complex(0, 0); - d_carr_error_hz = 0.0; - d_carr_error_filt_hz = 0.0; - d_code_error_chips = 0.0; - d_code_error_filt_chips = 0.0; - d_current_symbol = 0; -} - - -void galileo_e1_dll_pll_veml_tracking_cc::log_data() -{ - if (d_dump) - { - // Dump results to file - float prompt_I; - float prompt_Q; - float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; - float tmp_float; - double tmp_double; - - prompt_I = static_cast(d_P_accu.real()); - prompt_Q = static_cast(d_P_accu.imag()); - - tmp_VE = std::abs(d_VE_accu); - tmp_E = std::abs(d_E_accu); - tmp_P = std::abs(d_P_accu); - tmp_L = std::abs(d_L_accu); - tmp_VL = std::abs(d_VL_accu); - - try - { - // Dump correlators output - d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - tmp_float = d_acc_carrier_phase_rad; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // carrier and code frequency - tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands - tmp_float = d_carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // DLL commands - tmp_float = d_code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // CN0 and carrier lock test - tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } - } -} - - -int galileo_e1_dll_pll_veml_tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - // Block input data and block output stream pointers - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - switch (d_state) - { - case 0: // standby - bypass - { - current_synchro_data.Tracking_sample_counter = d_sample_counter; - break; - } - case 1: // pull-in - { - /* - * Signal alignment (skip samples until the incoming signal is aligned with local replica) - */ - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.fs = d_fs_in; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - consume_each(samples_offset); // shift input to perform alignment with local replica - d_state = 2; // next state is the symbol synchronization - return 0; - } - case 2: // wide tracking and symbol synchronization - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // save single correlation step variables - d_VE_accu = *d_Very_Early; - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - d_VL_accu = *d_Very_Late; - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - // perform DLL/PLL tracking loop computations - run_dll_pll(false); - - // ################## PLL COMMANDS ################################################# - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL COMMANDS ################################################# - // Code error from DLL - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; // [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_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - if (d_track_pilot) - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - - //std::cout<<(d_Prompt->real()>0); - if (d_enable_extended_integration) - { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - if (acquire_secondary() == true) - { - d_extend_correlation_symbols_count = 0; - // reset extended correlator - d_VE_accu = gr_complex(0, 0); - d_E_accu = gr_complex(0, 0); - d_P_accu = gr_complex(0, 0); - d_L_accu = gr_complex(0, 0); - d_VL_accu = gr_complex(0, 0); - d_Prompt_buffer_deque.clear(); - d_current_symbol = 0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; - - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - //std::cout << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl; - //std::cout << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; - - // UPDATE INTEGRATION TIME - double new_correlation_time_s = static_cast(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); - - d_state = 3; // next state is the extended correlator integrator - } - - d_Prompt_buffer_deque.pop_front(); - } - } - } - break; - } - case 3: // coherent integration (correlation time extension) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // correct the integration sign using the current symbol of the secondary code - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // PLL/DLL not enabled, we are in the middle of a coherent integration - // 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 - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## 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_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - break; - } - case 4: // narrow tracking - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step - do_correlation_step(in); - - // correct the integration using the current symbol - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - run_dll_pll(true); // Costas loop disabled, use four quadrant atan - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL ########################################################## - // Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[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_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - // reset extended correlator - d_VE_accu = gr_complex(0, 0); - d_E_accu = gr_complex(0, 0); - d_P_accu = gr_complex(0, 0); - d_L_accu = gr_complex(0, 0); - d_VL_accu = gr_complex(0, 0); - d_state = 3; //new coherent integration (correlation time extension) cycle - } - } - } - - //assign the GNURadio block output data - // current_synchro_data.System = {'E'}; - // std::string str_aux = "1B"; - // const char * str = str_aux.c_str(); // get a C style null terminated string - // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); - - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - - consume_each(d_current_prn_length_samples); // this is required for gr_block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - - if (current_synchro_data.Flag_valid_symbol_output) - { - return 1; - } - else - { - return 0; - } -} - - -int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() -{ - // READ DUMP FILE - std::ifstream::pos_type size; - int number_of_double_vars = 1; - int number_of_float_vars = 17; - int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + - sizeof(float) * number_of_float_vars + sizeof(unsigned int); - 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) - { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; - } - // count number of epochs and rewind - long int num_epoch = 0; - if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } - else - { - return 1; - } - float *abs_VE = new float[num_epoch]; - float *abs_E = new float[num_epoch]; - float *abs_P = new float[num_epoch]; - float *abs_L = new float[num_epoch]; - float *abs_VL = new float[num_epoch]; - float *Prompt_I = new float[num_epoch]; - float *Prompt_Q = new float[num_epoch]; - unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; - float *acc_carrier_phase_rad = new float[num_epoch]; - float *carrier_doppler_hz = new float[num_epoch]; - float *code_freq_chips = new float[num_epoch]; - float *carr_error_hz = new float[num_epoch]; - float *carr_error_filt_hz = new float[num_epoch]; - float *code_error_chips = new float[num_epoch]; - float *code_error_filt_chips = new float[num_epoch]; - float *CN0_SNV_dB_Hz = new float[num_epoch]; - float *carrier_lock_test = new float[num_epoch]; - float *aux1 = new float[num_epoch]; - double *aux2 = new double[num_epoch]; - unsigned int *PRN = new unsigned int[num_epoch]; - - try - { - if (dump_file.is_open()) - { - for (long int i = 0; i < num_epoch; i++) - { - dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); - dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); - } - } - dump_file.close(); - } - catch (const std::ifstream::failure &e) - { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 1; - } - - // WRITE MAT FILE - mat_t *matfp; - matvar_t *matvar; - std::string filename = d_dump_filename; - filename.erase(filename.length() - 4, 4); - filename.append(".mat"); - matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); - if (reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } - Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 0; -} - - -void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) -{ - d_channel = channel; - LOG(INFO) << "Tracking Channel set to " << d_channel; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(d_channel)); - d_dump_filename.append(".dat"); - d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } - } -} - - -void galileo_e1_dll_pll_veml_tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h deleted file mode 100644 index c82d01681..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ /dev/null @@ -1,244 +0,0 @@ -/*! - * \file galileo_e1_dll_pll_veml_tracking_cc.h - * \brief Implementation of a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals - * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H -#define GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H - -#include "gnss_synchro.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" -#include "cpu_multicorrelator_real_codes.h" -#include -#include -#include -#include - - -class galileo_e1_dll_pll_veml_tracking_cc; - -typedef boost::shared_ptr galileo_e1_dll_pll_veml_tracking_cc_sptr; - -galileo_e1_dll_pll_veml_tracking_cc_sptr -galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot); - -/*! - * \brief This class implements a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals - */ -class galileo_e1_dll_pll_veml_tracking_cc : public gr::block -{ -public: - ~galileo_e1_dll_pll_veml_tracking_cc(); - - void set_channel(unsigned int channel); - void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); - void start_tracking(); - - /*! - * \brief Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, - * Birkhauser, 2007 - */ - int general_work(int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - void forecast(int noutput_items, gr_vector_int &ninput_items_required); - -private: - friend galileo_e1_dll_pll_veml_tracking_cc_sptr - galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot); - - galileo_e1_dll_pll_veml_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot); - - bool cn0_and_tracking_lock_status(); - void do_correlation_step(const gr_complex *input_samples); - void run_dll_pll(bool disable_costas_loop); - void update_local_code(); - void update_local_carrier(); - bool acquire_secondary(); - - void clear_tracking_vars(); - - void log_data(); - - // tracking configuration vars - unsigned int d_vector_length; - bool d_dump; - - Gnss_Synchro *d_acquisition_gnss_synchro; - unsigned int d_channel; - long d_if_freq; - long d_fs_in; - - //tracking state machine - int d_state; - - //Integration period in samples - int d_correlation_length_samples; - int d_n_correlator_taps; - double d_early_late_spc_chips; - double d_very_early_late_spc_chips; - - double d_early_late_spc_narrow_chips; - double d_very_early_late_spc_narrow_chips; - - float *d_tracking_code; - float *d_data_code; - float *d_local_code_shift_chips; - gr_complex *d_correlator_outs; - cpu_multicorrelator_real_codes multicorrelator_cpu; - //todo: currently the multicorrelator does not support adding extra correlator - //with different local code, thus we need extra multicorrelator instance. - //Implement this functionality inside multicorrelator class - //as an enhancement to increase the performance - float *d_local_code_data_shift_chips; - cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel - - gr_complex *d_Very_Early; - gr_complex *d_Early; - gr_complex *d_Prompt; - gr_complex *d_Late; - gr_complex *d_Very_Late; - - int d_extend_correlation_symbols; - int d_extend_correlation_symbols_count; - bool d_enable_extended_integration; - int d_current_symbol; - - gr_complex d_VE_accu; - gr_complex d_E_accu; - gr_complex d_P_accu; - gr_complex d_L_accu; - gr_complex d_VL_accu; - - bool d_track_pilot; - gr_complex *d_Prompt_Data; - - double d_code_phase_step_chips; - double d_carrier_phase_step_rad; - // remaining code phase and carrier phase between tracking loops - double d_rem_code_phase_samples; - double d_rem_carr_phase_rad; - - // PLL and DLL filter library - Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; - - // acquisition - double d_acq_code_phase_samples; - double d_acq_carrier_doppler_hz; - - // tracking parameters - float d_dll_bw_hz; - float d_pll_bw_hz; - float d_dll_bw_narrow_hz; - float d_pll_bw_narrow_hz; - // tracking vars - double d_carr_error_hz; - double d_carr_error_filt_hz; - double d_code_error_chips; - double d_code_error_filt_chips; - - double d_K_blk_samples; - - double d_code_freq_chips; - double d_carrier_doppler_hz; - double d_acc_carrier_phase_rad; - double d_rem_code_phase_chips; - double d_code_phase_samples; - - //PRN period in samples - int d_current_prn_length_samples; - - //processing samples counters - unsigned long int d_sample_counter; - unsigned long int d_acq_sample_stamp; - - // CN0 estimation and lock detector - int d_cn0_estimation_counter; - std::deque d_Prompt_buffer_deque; - gr_complex *d_Prompt_buffer; - double d_carrier_lock_test; - double d_CN0_SNV_dB_Hz; - double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; - - // file dump - std::string d_dump_filename; - std::ofstream d_dump_file; - - std::map systemName; - std::string sys; - - int save_matfile(); -}; - -#endif //GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc deleted file mode 100644 index 494c4cfb4..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ /dev/null @@ -1,768 +0,0 @@ -/*! - * \file gps_l1_ca_dll_pll_tracking_cc.cc - * \brief Implementation of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es - * - * Code DLL + carrier PLL according to the algorithms described in: - * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkhauser, 2007 - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "gps_l1_ca_dll_pll_tracking_cc.h" -#include "gps_sdr_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "gnss_sdr_flags.h" -#include "GPS_L1_CA.h" -#include "control_message_factory.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using google::LogMessage; - -gps_l1_ca_dll_pll_tracking_cc_sptr -gps_l1_ca_dll_pll_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) -{ - return gps_l1_ca_dll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_cc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast(int noutput_items, - gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } -} - - -Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) : gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) -{ - // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); - this->message_port_register_out(pmt::mp("events")); - - // initialize internal vars - d_dump = dump; - d_if_freq = if_freq; - d_fs_in = fs_in; - d_vector_length = vector_length; - d_dump_filename = dump_filename; - - d_current_prn_length_samples = static_cast(d_vector_length); - - // Initialize tracking ========================================== - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); - - //--- DLL variables -------------------------------------------------------- - d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) - - // Initialization of local code replica - // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - - // correlator outputs (scalar) - d_n_correlator_taps = 3; // Early, Prompt, and Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_early_late_spc_chips; - 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); - - //--- Perform initializations ------------------------------ - // define initial code frequency basis of NCO - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; - // define residual code phase (in chips) - d_rem_code_phase_samples = 0.0; - // define residual carrier phase - d_rem_carr_phase_rad = 0.0; - - // sample synchronization - d_sample_counter = 0; - //d_sample_counter_seconds = 0; - d_acq_sample_stamp = 0; - - d_enable_tracking = false; - d_pull_in = false; - - // CN0 estimation and lock detector buffers - d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = FLAGS_carrier_lock_th; - - systemName["G"] = std::string("GPS"); - systemName["S"] = std::string("SBAS"); - - d_acquisition_gnss_synchro = 0; - d_channel = 0; - d_acq_code_phase_samples = 0.0; - d_acq_carrier_doppler_hz = 0.0; - d_carrier_doppler_hz = 0.0; - d_acc_carrier_phase_rad = 0.0; - d_code_phase_samples = 0.0; - d_rem_code_phase_chips = 0.0; - d_code_phase_step_chips = 0.0; - d_carrier_phase_step_rad = 0.0; - - set_relative_rate(1.0 / static_cast(d_vector_length)); -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() -{ - gr::thread::scoped_lock lk(d_setlock); - /* - * 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; - - long int acq_trk_diff_samples; - double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - // Doppler effect - // Fd=(C/(C+Vr))*F - 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; - d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; - d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1 / d_code_freq_chips; - T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); - - d_current_prn_length_samples = round(T_prn_mod_samples); - - double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(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; - double corrected_acq_phase_samples, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(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; - d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - - // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter - - // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); - - multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - 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; - - d_code_phase_samples = d_acq_code_phase_samples; - - std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); - - // DEBUG OUTPUT - 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) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking - d_pull_in = true; - d_enable_tracking = true; - - LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " Code Phase correction [samples]=" << delay_correction_samples - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; -} - - -int Gps_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() -{ - // READ DUMP FILE - std::ifstream::pos_type size; - int number_of_double_vars = 11; - int number_of_float_vars = 5; - int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + - sizeof(float) * number_of_float_vars + sizeof(unsigned int); - 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) - { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; - } - // count number of epochs and rewind - long int num_epoch = 0; - if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } - else - { - return 1; - } - float *abs_E = new float[num_epoch]; - float *abs_P = new float[num_epoch]; - float *abs_L = new float[num_epoch]; - float *Prompt_I = new float[num_epoch]; - float *Prompt_Q = new float[num_epoch]; - unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; - double *acc_carrier_phase_rad = new double[num_epoch]; - double *carrier_doppler_hz = new double[num_epoch]; - double *code_freq_chips = new double[num_epoch]; - double *carr_error_hz = new double[num_epoch]; - double *carr_error_filt_hz = new double[num_epoch]; - double *code_error_chips = new double[num_epoch]; - double *code_error_filt_chips = new double[num_epoch]; - double *CN0_SNV_dB_Hz = new double[num_epoch]; - double *carrier_lock_test = new double[num_epoch]; - double *aux1 = new double[num_epoch]; - double *aux2 = new double[num_epoch]; - unsigned int *PRN = new unsigned int[num_epoch]; - - try - { - if (dump_file.is_open()) - { - for (long int i = 0; i < num_epoch; i++) - { - dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); - dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); - } - } - dump_file.close(); - } - catch (const std::ifstream::failure &e) - { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 1; - } - - // WRITE MAT FILE - mat_t *matfp; - matvar_t *matvar; - std::string filename = d_dump_filename; - filename.erase(filename.length() - 4, 4); - filename.append(".mat"); - matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); - if (reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } - Mat_Close(matfp); - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 0; -} - - -Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc() -{ - if (d_dump_file.is_open()) - { - try - { - d_dump_file.close(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } - } - - if (d_dump) - { - if (d_channel == 0) - { - std::cout << "Writing .mat files ..."; - } - Gps_L1_Ca_Dll_Pll_Tracking_cc::save_matfile(); - if (d_channel == 0) - { - std::cout << " done." << std::endl; - } - } - - try - { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - gr::thread::scoped_lock lk(d_setlock); - // process vars - double carr_error_hz = 0.0; - double carr_error_filt_hz = 0.0; - double code_error_chips = 0.0; - double code_error_filt_chips = 0.0; - - // Block input data and block output stream pointers - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - - // 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) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - d_pull_in = false; - // take into account the carrier cycles accumulated in the pull in signal alignment - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; - 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; - consume_each(samples_offset); // shift input to perform alignment with local replica - return 0; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // ################## PLL ########################################################## - // PLL discriminator - // Update PLL discriminator [rads/Ti -> Secs/Ti] - carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] - double T_chip_seconds = 1.0 / static_cast(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] - //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [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_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - //################### PLL COMMANDS ################################################# - // carrier phase step (NCO phase increment per sample) [rads/sample] - d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); - // carrier phase accumulator - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; - - //################### DLL COMMANDS ################################################# - // code phase step (Code resampler phase increment per sample) [chips/sample] - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - 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(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < FLAGS_cn0_samples) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = 1; - } - else - { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - current_synchro_data.System = {'G'}; - current_synchro_data.correlation_length_ms = 1; - } - - //assign the GNURadio block output data - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - if (d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - unsigned long int tmp_long; - prompt_I = d_correlator_outs[1].real(); - prompt_Q = d_correlator_outs[1].imag(); - tmp_E = std::abs(d_correlator_outs[0]); - tmp_P = std::abs(d_correlator_outs[1]); - tmp_L = std::abs(d_correlator_outs[2]); - try - { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - // PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - - // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - - // AUX vars (for debug purposes) - tmp_double = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } - catch (const std::ifstream::failure &e) - { - 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 - - if (current_synchro_data.Flag_valid_symbol_output) - { - return 1; - } - else - { - return 0; - } -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) -{ - d_channel = channel; - LOG(INFO) << "Tracking Channel set to " << d_channel; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(d_channel)); - d_dump_filename.append(".dat"); - d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } - } -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h deleted file mode 100644 index 575b4feb4..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ /dev/null @@ -1,167 +0,0 @@ -/*! - * \file gps_l1_ca_dll_pll_tracking_cc.h - * \brief Interface of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es - * Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com - * - * Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, - * Birkhauser, 2007 - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H -#define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H - -#include "gnss_synchro.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" -#include "cpu_multicorrelator_real_codes.h" -#include -#include -#include -#include - -class Gps_L1_Ca_Dll_Pll_Tracking_cc; - -typedef boost::shared_ptr - gps_l1_ca_dll_pll_tracking_cc_sptr; - -gps_l1_ca_dll_pll_tracking_cc_sptr -gps_l1_ca_dll_pll_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - -/*! - * \brief This class implements a DLL + PLL tracking loop block - */ -class Gps_L1_Ca_Dll_Pll_Tracking_cc : public gr::block -{ -public: - ~Gps_L1_Ca_Dll_Pll_Tracking_cc(); - - void set_channel(unsigned int channel); - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); - void start_tracking(); - - int general_work(int noutput_items, gr_vector_int& ninput_items, - gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); - - void forecast(int noutput_items, gr_vector_int& ninput_items_required); - -private: - friend gps_l1_ca_dll_pll_tracking_cc_sptr - gps_l1_ca_dll_pll_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - Gps_L1_Ca_Dll_Pll_Tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - int save_matfile(); - // tracking configuration vars - unsigned int d_vector_length; - bool d_dump; - - Gnss_Synchro* d_acquisition_gnss_synchro; - unsigned int d_channel; - - long d_if_freq; - long d_fs_in; - - double d_early_late_spc_chips; - - // remaining code phase and carrier phase between tracking loops - double d_rem_code_phase_samples; - double d_rem_code_phase_chips; - double d_rem_carr_phase_rad; - - // PLL and DLL filter library - Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; - - // acquisition - double d_acq_code_phase_samples; - double d_acq_carrier_doppler_hz; - // correlator - int d_n_correlator_taps; - float* d_ca_code; - float* d_local_code_shift_chips; - gr_complex* d_correlator_outs; - cpu_multicorrelator_real_codes multicorrelator_cpu; - - // tracking vars - double d_code_freq_chips; - double d_code_phase_step_chips; - double d_carrier_doppler_hz; - double d_carrier_phase_step_rad; - double d_acc_carrier_phase_rad; - double d_code_phase_samples; - - //PRN period in samples - int d_current_prn_length_samples; - - //processing samples counters - unsigned long int d_sample_counter; - unsigned long int d_acq_sample_stamp; - - // CN0 estimation and lock detector - int d_cn0_estimation_counter; - gr_complex* d_Prompt_buffer; - double d_carrier_lock_test; - double d_CN0_SNV_dB_Hz; - double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; - - // control vars - bool d_enable_tracking; - bool d_pull_in; - - // file dump - std::string d_dump_filename; - std::ofstream d_dump_file; - - std::map systemName; - std::string sys; -}; - -#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index 7ed7741da..f21ae5811 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -228,7 +228,6 @@ void GpsL1CADllPllTrackingTest::configure_receiver() config->set_property("Tracking_1C.pll_bw_narrow_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_narrow_hz", "2.0"); config->set_property("Tracking_1C.early_late_space_narrow_chips", "0.5"); - config->set_property("Tracking_1C.unified", "true"); config->set_property("Tracking_1C.extend_correlation_ms", "1"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");