diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index d9ef75ae3..4b7aae6d0 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -34,16 +34,15 @@ * ------------------------------------------------------------------------- */ +#include "gps_l1_ca_pcps_acquisition_fpga.h" #include "configuration_interface.h" #include "gnss_sdr_flags.h" -#include "gps_l1_ca_pcps_acquisition_fpga.h" -#include "gps_sdr_signal_processing.h" #include "GPS_L1_CA.h" +#include "gps_sdr_signal_processing.h" #include #include #include - #define NUM_PRNs 32 using google::LogMessage; @@ -123,8 +122,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( } } - //acq_parameters - + // acq_parameters acq_parameters.all_fft_codes = d_all_fft_codes_; // temporary buffers that we can delete @@ -132,7 +130,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( delete fft_if; delete[] fft_codes_padded; - acquisition_fpga_ = pcps_make_acquisition(acq_parameters); + acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); DLOG(INFO) << "acquisition(" << acquisition_fpga_->unique_id() << ")"; channel_ = 0; @@ -211,15 +209,20 @@ void GpsL1CaPcpsAcquisitionFpga::set_state(int state) acquisition_fpga_->set_state(state); } + void GpsL1CaPcpsAcquisitionFpga::connect(gr::top_block_sptr top_block) { - // nothing to connect + if (top_block) + { // nothing to disconnect + } } void GpsL1CaPcpsAcquisitionFpga::disconnect(gr::top_block_sptr top_block) { - // nothing to disconnect + if (top_block) + { // nothing to disconnect + } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 6a337925e..8a5d3ea99 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -38,22 +38,22 @@ * ------------------------------------------------------------------------- */ - +#include "pcps_acquisition_fpga.h" #include #include -#include "pcps_acquisition_fpga.h" + using google::LogMessage; -pcps_acquisition_fpga_sptr pcps_make_acquisition(pcpsconf_fpga_t conf_) +pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_) { return pcps_acquisition_fpga_sptr(new pcps_acquisition_fpga(conf_)); } pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) : gr::block("pcps_acquisition_fpga", - gr::io_signature::make(0, 0, 0), - gr::io_signature::make(0, 0, 0)) + gr::io_signature::make(0, 0, 0), + gr::io_signature::make(0, 0, 0)) { this->message_port_register_out(pmt::mp("events")); @@ -71,10 +71,8 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) : gr::block( d_channel = 0; d_gnss_synchro = 0; - acquisition_fpga = std::make_shared - (acq_parameters.device_name, d_fft_size, acq_parameters.doppler_max, acq_parameters.samples_per_ms, - acq_parameters.fs_in, acq_parameters.freq, acq_parameters.sampled_ms, acq_parameters.select_queue_Fpga, acq_parameters.all_fft_codes); - + acquisition_fpga = std::make_shared(acq_parameters.device_name, d_fft_size, acq_parameters.doppler_max, acq_parameters.samples_per_ms, + acq_parameters.fs_in, acq_parameters.freq, acq_parameters.sampled_ms, acq_parameters.select_queue_Fpga, acq_parameters.all_fft_codes); } @@ -196,9 +194,9 @@ void pcps_acquisition_fpga::set_active(bool active) int doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * doppler_index; acquisition_fpga->set_phase_step(doppler_index); - acquisition_fpga->run_acquisition(); // runs acquisition and waits until it is finished + acquisition_fpga->run_acquisition(); // runs acquisition and waits until it is finished acquisition_fpga->read_acquisition_results(&indext, &magt, - &initial_sample, &d_input_power); + &initial_sample, &d_input_power); d_sample_counter = initial_sample; if (d_mag < magt) @@ -213,7 +211,7 @@ void pcps_acquisition_fpga::set_active(bool active) d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; - d_test_statistics = (d_mag / d_input_power); //* correction_factor; + d_test_statistics = (d_mag / d_input_power); //* correction_factor; } // In the case of the FPGA the option of dumping the results of the acquisition to a file is not available diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index e758904e3..36ddcd70f 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -81,7 +81,7 @@ class pcps_acquisition_fpga; typedef boost::shared_ptr pcps_acquisition_fpga_sptr; pcps_acquisition_fpga_sptr -pcps_make_acquisition(pcpsconf_fpga_t conf_); +pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); /*! * \brief This class implements a Parallel Code Phase Search Acquisition that uses the FPGA. @@ -94,7 +94,7 @@ class pcps_acquisition_fpga : public gr::block private: friend pcps_acquisition_fpga_sptr - pcps_make_acquisition(pcpsconf_fpga_t conf_); + pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); pcps_acquisition_fpga(pcpsconf_fpga_t conf_); diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 77f667407..111de94c5 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -1,8 +1,9 @@ /*! * \file gps_l1_ca_dll_pll_tracking.cc * \brief Implementation of an adapter of a DLL+PLL tracking loop block - * for GPS L1 C/A to a TrackingInterface - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * for GPS L1 C/A to a TrackingInterface that uses the FPGA + * \author Marc Majoral, 2018, mmajoral(at)cttc.es + * 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: @@ -35,65 +36,109 @@ * ------------------------------------------------------------------------- */ - #include "gps_l1_ca_dll_pll_tracking_fpga.h" #include "configuration_interface.h" +#include "display.h" +#include "gnss_sdr_flags.h" #include "GPS_L1_CA.h" +#include "gps_sdr_signal_processing.h" #include +#define NUM_PRNs 32 + using google::LogMessage; GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( ConfigurationInterface* configuration, std::string role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) { + dllpllconf_fpga_t trk_param_fpga; DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; - //std::string default_item_type = "gr_complex"; - std::string default_item_type = "cshort"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - std::string device_name; - unsigned int device_base; - std::string default_device_name = "/dev/uio"; - device_name = configuration->property(role + ".devicename", default_device_name); - device_base = configuration->property(role + ".device_base", 1); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + trk_param_fpga.fs_in = fs_in; + bool dump = configuration->property(role + ".dump", false); + trk_param_fpga.dump = dump; + 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); + trk_param_fpga.pll_bw_hz = pll_bw_hz; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); + trk_param_fpga.pll_bw_narrow_hz = pll_bw_narrow_hz; + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); + trk_param_fpga.dll_bw_narrow_hz = dll_bw_narrow_hz; + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + trk_param_fpga.dll_bw_hz = dll_bw_hz; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + trk_param_fpga.early_late_space_chips = early_late_space_chips; + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); + trk_param_fpga.early_late_space_narrow_chips = early_late_space_narrow_chips; std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! - vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); - if (item_type.compare("cshort") == 0) + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + trk_param_fpga.dump_filename = dump_filename; + int vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + trk_param_fpga.vector_length = vector_length; + int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + if (symbols_extended_correlator < 1) { - item_size_ = sizeof(lv_16sc_t); - tracking_fpga_sc = gps_l1_ca_dll_pll_make_tracking_fpga_sc( - f_if, fs_in, vector_length, dump, dump_filename, pll_bw_hz, - dll_bw_hz, early_late_space_chips, device_name, - device_base); - DLOG(INFO) << "tracking(" << tracking_fpga_sc->unique_id() - << ")"; + symbols_extended_correlator = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; } - else + else if (symbols_extended_correlator > 20) { - item_size_ = sizeof(lv_16sc_t); - // LOG(WARNING) << item_type_ << " unknown tracking item type"; - LOG(WARNING) << item_type - << " the tracking item type for the FPGA tracking test has to be cshort"; + symbols_extended_correlator = 20; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } + trk_param_fpga.extend_correlation_symbols = symbols_extended_correlator; + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (track_pilot) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; + } + if ((symbols_extended_correlator > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } + trk_param_fpga.very_early_late_space_chips = 0.0; + trk_param_fpga.very_early_late_space_narrow_chips = 0.0; + trk_param_fpga.track_pilot = false; + trk_param_fpga.system = 'G'; + char sig_[3] = "1C"; + std::memcpy(trk_param_fpga.signal, sig_, 3); + int cn0_samples = configuration->property(role + ".cn0_samples", 20); + if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; + trk_param_fpga.cn0_samples = cn0_samples; + int cn0_min = configuration->property(role + ".cn0_min", 25); + if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; + trk_param_fpga.cn0_min = cn0_min; + int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; + trk_param_fpga.max_lock_fail = max_lock_fail; + double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); + if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; + trk_param_fpga.carrier_lock_th = carrier_lock_th; + + // FPGA configuration parameters + std::string default_device_name = "/dev/uio"; + std::string device_name = configuration->property(role + ".devicename", default_device_name); + trk_param_fpga.device_name = device_name; + unsigned int device_base = configuration->property(role + ".device_base", 1); + trk_param_fpga.device_base = device_base; + + //################# PRE-COMPUTE ALL THE CODES ################# + d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); + for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) + { + gps_l1_ca_code_gen_int(&d_ca_codes[(int(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); + } + trk_param_fpga.ca_codes = d_ca_codes; + trk_param_fpga.code_length = GPS_L1_CA_CODE_LENGTH_CHIPS; + + //################# MAKE TRACKING GNURadio object ################### + tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; DLOG(INFO) << "tracking(" << tracking_fpga_sc->unique_id() << ")"; } @@ -101,6 +146,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( GpsL1CaDllPllTrackingFpga::~GpsL1CaDllPllTrackingFpga() { + delete[] d_ca_codes; } @@ -131,7 +177,7 @@ void GpsL1CaDllPllTrackingFpga::connect(gr::top_block_sptr top_block) if (top_block) { /* top_block is not null */ }; - //nothing to connect, now the tracking uses gr_sync_decimator + //nothing to connect } @@ -140,7 +186,7 @@ void GpsL1CaDllPllTrackingFpga::disconnect(gr::top_block_sptr top_block) if (top_block) { /* top_block is not null */ }; - //nothing to disconnect, now the tracking uses gr_sync_decimator + //nothing to disconnect } @@ -154,9 +200,3 @@ gr::basic_block_sptr GpsL1CaDllPllTrackingFpga::get_right_block() { return tracking_fpga_sc; } - - -void GpsL1CaDllPllTrackingFpga::reset(void) -{ - // tracking_fpga_sc->reset(); -} diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index 0abb88220..4c5a171d2 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -1,8 +1,9 @@ /*! * \file gps_l1_ca_dll_pll_tracking.h * \brief Interface of an adapter of a DLL+PLL tracking loop block - * for GPS L1 C/A to a TrackingInterface - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com + * for GPS L1 C/A to a TrackingInterface that uses the FPGA + * \author Marc Majoral, 2018. mmajoral(at)cttc.es + * 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: @@ -38,9 +39,8 @@ #ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_H_ #define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_H_ - #include "tracking_interface.h" -#include "gps_l1_ca_dll_pll_tracking_fpga_sc.h" +#include "dll_pll_veml_tracking_fpga.h" #include class ConfigurationInterface; @@ -92,16 +92,14 @@ public: void start_tracking() override; - void reset(void); - private: - //gps_l1_ca_dll_pll_tracking_cc_sptr tracking_; - gps_l1_ca_dll_pll_tracking_fpga_sc_sptr tracking_fpga_sc; + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + int* d_ca_codes; }; #endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 4cb869a79..2010ef817 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -23,7 +23,7 @@ if(ENABLE_CUDA) endif(ENABLE_CUDA) if(ENABLE_FPGA) - set(OPT_TRACKING_BLOCKS ${OPT_TRACKING_BLOCKS} gps_l1_ca_dll_pll_tracking_fpga_sc.cc) + set(OPT_TRACKING_BLOCKS ${OPT_TRACKING_BLOCKS} dll_pll_veml_tracking_fpga.cc) endif(ENABLE_FPGA) set(TRACKING_GR_BLOCKS_SOURCES diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc new file mode 100644 index 000000000..fe856cded --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -0,0 +1,1474 @@ +/*! + * \file dll_pll_veml_tracking.cc + * \brief Implementation of a code DLL + carrier PLL tracking block using an FPGA + * \author Marc Majoral, 2018. marc.majoral(at)cttc.es + * Antonio Ramos, 2018 antonio.ramosdet(at)gmail.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-2018 (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 "dll_pll_veml_tracking_fpga.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "control_message_factory.h" +#include "MATH_CONSTANTS.h" +#include "Galileo_E1.h" +#include "Galileo_E5a.h" +#include "GPS_L1_CA.h" +#include "GPS_L2C.h" +#include "gps_l2c_signal.h" +#include "GPS_L5.h" +#include "gps_l5_signal.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using google::LogMessage; + +dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(dllpllconf_fpga_t conf_) +{ + return dll_pll_veml_tracking_fpga_sptr(new dll_pll_veml_tracking_fpga(conf_)); +} + + +dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(dllpllconf_fpga_t conf_) : gr::block("dll_pll_veml_tracking_fpga", gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + trk_parameters = conf_; + // Telemetry bit synchronization message port input + this->message_port_register_out(pmt::mp("events")); + this->set_relative_rate(1.0 / static_cast(trk_parameters.vector_length)); + + // initialize internal vars + d_veml = false; + d_cloop = true; + d_synchonizing = false; + d_code_chip_rate = 0.0; + d_secondary_code_length = 0; + d_secondary_code_string = nullptr; + signal_type = std::string(trk_parameters.signal); + + std::map map_signal_pretty_name; + map_signal_pretty_name["1C"] = "L1 C/A"; + map_signal_pretty_name["1B"] = "E1"; + map_signal_pretty_name["1G"] = "L1 C/A"; + map_signal_pretty_name["2S"] = "L2C"; + map_signal_pretty_name["2G"] = "L2 C/A"; + map_signal_pretty_name["5X"] = "E5a"; + map_signal_pretty_name["L5"] = "L5"; + + signal_pretty_name = map_signal_pretty_name[signal_type]; + + if (trk_parameters.system == 'G') + { + systemName = "GPS"; + if (signal_type.compare("1C") == 0) + { + d_signal_carrier_freq = GPS_L1_FREQ_HZ; + d_code_period = GPS_L1_CA_CODE_PERIOD; + d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + // GPS L1 C/A does not have pilot component nor secondary code + d_secondary = false; + trk_parameters.track_pilot = false; + interchange_iq = false; + } + else if (signal_type.compare("2S") == 0) + { + d_signal_carrier_freq = GPS_L2_FREQ_HZ; + d_code_period = GPS_L2_M_PERIOD; + d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 20; + d_code_samples_per_chip = 1; + // GPS L2 does not have pilot component nor secondary code + d_secondary = false; + trk_parameters.track_pilot = false; + interchange_iq = false; + } + else if (signal_type.compare("L5") == 0) + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5i_PERIOD; + d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; + d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); + d_secondary = true; + if (trk_parameters.track_pilot) + { + d_secondary_code_length = static_cast(GPS_L5q_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5q_NH_CODE_STR); + signal_pretty_name = signal_pretty_name + "Q"; + interchange_iq = true; + } + else + { + d_secondary_code_length = static_cast(GPS_L5i_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5i_NH_CODE_STR); + signal_pretty_name = signal_pretty_name + "I"; + interchange_iq = false; + } + } + else + { + 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 (trk_parameters.system == 'E') + { + systemName = "Galileo"; + if (signal_type.compare("1B") == 0) + { + d_signal_carrier_freq = Galileo_E1_FREQ_HZ; + d_code_period = Galileo_E1_CODE_PERIOD; + d_code_chip_rate = Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_length_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + d_symbols_per_bit = 1; + d_correlation_length_ms = 4; + d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip + d_veml = true; + if (trk_parameters.track_pilot) + { + d_secondary = true; + d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + signal_pretty_name = signal_pretty_name + "C"; + } + else + { + d_secondary = false; + signal_pretty_name = signal_pretty_name + "B"; + } + interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + } + else if (signal_type.compare("5X") == 0) + { + d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; + d_code_period = GALILEO_E5a_CODE_PERIOD; + d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; + d_symbols_per_bit = 20; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); + d_secondary = true; + if (trk_parameters.track_pilot) + { + d_secondary_code_length = static_cast(Galileo_E5a_Q_SECONDARY_CODE_LENGTH); + signal_pretty_name = signal_pretty_name + "Q"; + interchange_iq = true; + } + else + { + d_secondary_code_length = static_cast(Galileo_E5a_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E5a_I_SECONDARY_CODE); + signal_pretty_name = signal_pretty_name + "I"; + interchange_iq = false; + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + 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 + { + LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; + std::cerr << "Invalid System 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; + } + T_chip_seconds = 0.0; + T_prn_seconds = 0.0; + T_prn_samples = 0.0; + K_blk_samples = 0.0; + + // Initialize tracking ========================================== + d_code_loop_filter = Tracking_2nd_DLL_filter(static_cast(d_code_period)); + d_carrier_loop_filter = Tracking_2nd_PLL_filter(static_cast(d_code_period)); + d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); + d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); + + if (d_veml) + { + // Very-Early, Early, Prompt, Late, Very-Late + d_n_correlator_taps = 5; + } + else + { + // Early, Prompt, Late + d_n_correlator_taps = 3; + } + + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + // map memory pointers of correlator outputs + if (d_veml) + { + 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[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[2]; + } + else + { + d_Very_Early = nullptr; + d_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + d_Very_Late = nullptr; + d_local_code_shift_chips[0] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[1]; + } + + if (trk_parameters.extend_correlation_symbols > 1) + { + d_enable_extended_integration = true; + } + else + { + d_enable_extended_integration = false; + trk_parameters.extend_correlation_symbols = 1; + } + + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + if (trk_parameters.track_pilot) + { + // Extra correlator for the data component + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + else + { + d_Prompt_Data = nullptr; + } + + //--- Initializations ---// + // Initial code frequency basis of NCO + d_code_freq_chips = d_code_chip_rate; + // 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_acq_sample_stamp = 0; + + d_current_prn_length_samples = static_cast(trk_parameters.vector_length); + d_next_prn_length_samples = d_current_prn_length_samples; + d_correlation_length_samples = static_cast(trk_parameters.vector_length); // this one is only for initialisation and does not change its value (MM) + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples]; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = trk_parameters.carrier_lock_th; + + clear_tracking_vars(); + + d_acquisition_gnss_synchro = nullptr; + 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_last_prompt = gr_complex(0.0, 0.0); + d_state = 0; // initial state: standby + + // create multicorrelator class + std::string device_name = trk_parameters.device_name; + unsigned int device_base = trk_parameters.device_base; + int *ca_codes = trk_parameters.ca_codes; + unsigned int code_length = trk_parameters.code_length; + multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, code_length); + multicorrelator_fpga->set_output_vectors(d_correlator_outs); + + d_pull_in = 0; +} + +void dll_pll_veml_tracking_fpga::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; + + double acq_trk_diff_seconds = 0; // when using the FPGA we don't use the global sample counter + // Doppler effect Fd = (C / (C + Vr)) * F + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; + // new chip and prn sequence periods based on acq Doppler + d_code_freq_chips = radial_velocity * d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + double T_chip_mod_seconds = 1.0 / d_code_freq_chips; + double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); + double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in; + + d_current_prn_length_samples = std::round(T_prn_mod_samples); + d_next_prn_length_samples = d_current_prn_length_samples; + double T_prn_true_seconds = static_cast(d_code_length_chips) / d_code_chip_rate; + double T_prn_true_samples = T_prn_true_seconds * trk_parameters.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 = std::fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * trk_parameters.fs_in, T_prn_true_samples); + if (corrected_acq_phase_samples < 0.0) + { + corrected_acq_phase_samples += T_prn_mod_samples; + } + double 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 = PI_2 * d_carrier_doppler_hz / trk_parameters.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 (systemName.compare("GPS") == 0 and signal_type.compare("1C") == 0) + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + else if (systemName.compare("GPS") == 0 and signal_type.compare("2S") == 0) + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + else if (systemName.compare("GPS") == 0 and signal_type.compare("L5") == 0) + { + if (trk_parameters.track_pilot) + { + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + else + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("1B") == 0) + { + if (trk_parameters.track_pilot) + { + char pilot_signal[3] = "1C"; + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + else + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("5X") == 0) + { + if (trk_parameters.track_pilot) + { + d_secondary_code_string = const_cast(&Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + else + { + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + // nothing to compute : the local codes are pre-computed in the adapter class + } + } + } + + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_cn0_estimation_counter = 0; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + } + + d_code_phase_samples = d_acq_code_phase_samples; + d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); + d_carrier_loop_filter.set_pdi(static_cast(d_code_period)); + d_code_loop_filter.set_pdi(static_cast(d_code_period)); + + // DEBUG OUTPUT + std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + d_synchonizing = false; + d_cloop = true; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); + 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; + multicorrelator_fpga->set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_local_code_shift_chips, d_acquisition_gnss_synchro->PRN); + d_pull_in = 1; + // enable tracking pull-in and d_state at the end to avoid general work from starting pull-in before the start tracking function is finished + d_state = 1; +} + +dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } + if (trk_parameters.dump) + { + if (d_channel == 0) + { + std::cout << "Writing .mat files ..."; + } + 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); + if (trk_parameters.track_pilot) + { + volk_gnsssdr_free(d_Prompt_Data); + } + delete[] d_Prompt_buffer; + multicorrelator_fpga->free(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + +bool dll_pll_veml_tracking_fpga::acquire_secondary() +{ + // ******* preamble correlation ******** + int corr_value = 0; + for (unsigned int i = 0; i < d_secondary_code_length; i++) + { + if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping + { + if (d_secondary_code_string->at(i) == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } + else + { + if (d_secondary_code_string->at(i) == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } + } + + if (abs(corr_value) == d_secondary_code_length) + { + return true; + } + else + { + return false; + } +} + + +bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integration_time_s) +{ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < trk_parameters.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, trk_parameters.cn0_samples, coh_integration_time_s); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.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 > trk_parameters.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; + multicorrelator_fpga->unlock_channel(); + return false; + } + else + { + return true; + } + } +} + + +void dll_pll_veml_tracking_fpga::run_dll_pll() +{ + // ################## PLL ########################################################## + // PLL discriminator + if (d_cloop) + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; + } + else + { + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; + } + + // 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 = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate; + + // ################## DLL ########################################################## + // DLL discriminator + if (d_veml) + { + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + } + else + { + d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] + } + // Code discriminator filter + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] +} + + +void dll_pll_veml_tracking_fpga::clear_tracking_vars() +{ + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + if (trk_parameters.track_pilot) *d_Prompt_Data = gr_complex(0.0, 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; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); +} + + +void dll_pll_veml_tracking_fpga::update_tracking_vars() +{ + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double code_error_filt_secs = T_prn_seconds * d_code_error_filt_chips * T_chip_seconds; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + T_prn_samples = T_prn_seconds * trk_parameters.fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * trk_parameters.fs_in; + d_next_prn_length_samples = round(K_blk_samples); + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(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 / trk_parameters.fs_in; + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; +} + + +void dll_pll_veml_tracking_fpga::save_correlation_results() +{ + if (d_secondary) + { + if (d_secondary_code_string->at(d_current_symbol) == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + } + else + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_current_symbol++; + d_current_symbol %= d_symbols_per_bit; + } + // If tracking pilot, disable Costas loop + if (trk_parameters.track_pilot) + d_cloop = false; + else + d_cloop = true; +} + + +void dll_pll_veml_tracking_fpga::log_data(bool integrating) +{ + if (trk_parameters.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; + if (trk_parameters.track_pilot) + { + if (interchange_iq) + { + prompt_I = d_Prompt_Data->imag(); + prompt_Q = d_Prompt_Data->real(); + } + else + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } + } + else + { + if (interchange_iq) + { + prompt_I = d_Prompt->imag(); + prompt_Q = d_Prompt->real(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } + } + if (d_veml) + { + tmp_VE = std::abs(d_VE_accu); + tmp_VL = std::abs(d_VL_accu); + } + else + { + tmp_VE = 0.0; + tmp_VL = 0.0; + } + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + if (integrating) + { + //TODO: Improve this solution! + // It compensates the amplitude difference while integrating + if (d_extend_correlation_symbols_count > 0) + { + float scale_factor = static_cast(trk_parameters.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 + { + // 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 dll_pll_veml_tracking_fpga::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(trk_parameters.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 = trk_parameters.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_VE, 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_VL, 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 dll_pll_veml_tracking_fpga::set_channel(unsigned int channel) +{ + d_channel = channel; + multicorrelator_fpga->set_channel(d_channel); + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (trk_parameters.dump) + { + if (!d_dump_file.is_open()) + { + try + { + trk_parameters.dump_filename.append(boost::lexical_cast(d_channel)); + trk_parameters.dump_filename.append(".dat"); + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(trk_parameters.dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << trk_parameters.dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} + + +void dll_pll_veml_tracking_fpga::reset(void) +{ + multicorrelator_fpga->unlock_channel(); +} + + +int dll_pll_veml_tracking_fpga::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 + 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(); + + d_current_prn_length_samples = d_next_prn_length_samples; + current_synchro_data = *d_acquisition_gnss_synchro; + + switch (d_state) + { + case 0: // Standby - Consume samples at full throttle, do nothing + { + 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; + break; + } + case 1: // Standby - Consume samples at full throttle, do nothing + { + d_pull_in = 0; + multicorrelator_fpga->lock_channel(); + unsigned counter_value = multicorrelator_fpga->read_sample_counter(); + unsigned num_frames = ceil((counter_value - current_synchro_data.Acq_samplestamp_samples - current_synchro_data.Acq_delay_samples) / d_correlation_length_samples); + unsigned absolute_samples_offset = current_synchro_data.Acq_delay_samples + current_synchro_data.Acq_samplestamp_samples + num_frames * d_correlation_length_samples; + multicorrelator_fpga->set_initial_sample(absolute_samples_offset); + d_sample_counter = absolute_samples_offset; + current_synchro_data.Tracking_sample_counter = absolute_samples_offset; + d_sample_counter_next = d_sample_counter; + d_state = 2; + return 0; + break; + } + + case 2: + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + d_current_prn_length_samples; + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_fpga->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); + + // Save single correlation step variables + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + if (!cn0_and_tracking_lock_status(d_code_period)) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + bool next_state = false; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + + d_Prompt_buffer_deque.pop_front(); + } + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { + if (d_synchonizing) + { + if (d_Prompt->real() * d_last_prompt.real() > 0.0) + { + d_current_symbol++; + } + else if (d_current_symbol > d_symbols_per_bit) + { + d_synchonizing = false; + d_current_symbol = 1; + } + else + { + d_current_symbol = 1; + d_last_prompt = *d_Prompt; + } + } + else if (d_last_prompt.real() != 0.0) + { + d_current_symbol++; + if (d_current_symbol == d_symbols_per_bit) next_state = true; + } + else + { + d_last_prompt = *d_Prompt; + d_synchonizing = true; + d_current_symbol = 1; + } + } + else + { + next_state = true; + } + + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_last_prompt = gr_complex(0.0, 0.0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_synchonizing = false; + + if (d_enable_extended_integration) + { + // UPDATE INTEGRATION TIME + d_extend_correlation_symbols_count = 0; + float new_correlation_time = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time); + d_code_loop_filter.set_pdi(new_correlation_time); + d_state = 3; // next state is the extended correlator integrator + LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_narrow_hz); + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + } + } + else + { + d_state = 4; + } + } + } + + break; + } + + case 3: + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + d_current_prn_length_samples; + + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + multicorrelator_fpga->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); + update_tracking_vars(); + save_correlation_results(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + 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.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 = d_correlation_length_ms; + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + log_data(true); + break; + } + + case 4: // narrow tracking + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + d_current_prn_length_samples; + + // perform a correlation step + //do_correlation_step(in); + multicorrelator_fpga->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); + + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(); + update_tracking_vars(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + 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.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 = d_correlation_length_ms; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; // new coherent integration (correlation time extension) cycle + } + } + } + } + if (current_synchro_data.Flag_valid_symbol_output) + { + current_synchro_data.fs = static_cast(trk_parameters.fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + *out[0] = current_synchro_data; + return 1; + } + return 0; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h new file mode 100644 index 000000000..0d8b93f1e --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -0,0 +1,226 @@ +/*! + * \file gps_l1_ca_dll_pll_tracking_fpga.h + * \brief Interface of a code DLL + carrier PLL tracking block + * \author Marc Majoral, 2018. marc.majoral(at)cttc.es + * 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_DLL_PLL_VEML_TRACKING_FPGA_H +#define GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H + +#include "fpga_multicorrelator.h" +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include +#include +#include +#include + + +typedef struct +{ + /* DLL/PLL tracking configuration */ + double 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; + int cn0_samples; + int cn0_min; + int max_lock_fail; + double carrier_lock_th; + bool track_pilot; + char system; + char signal[3]; + std::string device_name; + unsigned int device_base; + unsigned int code_length; + int *ca_codes; +} dllpllconf_fpga_t; + +class dll_pll_veml_tracking_fpga; + +typedef boost::shared_ptr + dll_pll_veml_tracking_fpga_sptr; + +dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(dllpllconf_fpga_t conf_); + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class dll_pll_veml_tracking_fpga : public gr::block +{ +public: + ~dll_pll_veml_tracking_fpga(); + + 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 reset(void); + +private: + friend dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(dllpllconf_fpga_t conf_); + + dll_pll_veml_tracking_fpga(dllpllconf_fpga_t conf_); + + bool cn0_and_tracking_lock_status(double coh_integration_time_s); + bool acquire_secondary(); + void run_dll_pll(); + void update_tracking_vars(); + void clear_tracking_vars(); + void save_correlation_results(); + void log_data(bool integrating); + int save_matfile(); + + // tracking configuration vars + dllpllconf_fpga_t trk_parameters; + bool d_veml; + bool d_cloop; + unsigned int d_channel; + Gnss_Synchro *d_acquisition_gnss_synchro; + + //Signal parameters + bool d_secondary; + bool interchange_iq; + double d_signal_carrier_freq; + double d_code_period; + double d_code_chip_rate; + unsigned int d_secondary_code_length; + unsigned int d_code_length_chips; + unsigned int d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) + int d_symbols_per_bit; + std::string systemName; + std::string signal_type; + std::string *d_secondary_code_string; + std::string signal_pretty_name; + + //tracking state machine + int d_state; + bool d_synchonizing; + //Integration period in samples + int d_correlation_length_ms; + int d_n_correlator_taps; + float *d_local_code_shift_chips; + float *d_prompt_data_shift; + std::shared_ptr multicorrelator_fpga; + + gr_complex *d_correlator_outs; + gr_complex *d_Very_Early; + gr_complex *d_Early; + gr_complex *d_Prompt; + gr_complex *d_Late; + gr_complex *d_Very_Late; + + bool d_enable_extended_integration; + int d_extend_correlation_symbols_count; + 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; + gr_complex d_last_prompt; + + 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 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; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_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; + int d_carrier_lock_fail_counter; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + std::deque d_Prompt_buffer_deque; + gr_complex *d_Prompt_buffer; + + // file dump + std::ofstream d_dump_file; + + // extra + int d_correlation_length_samples; + int d_next_prn_length_samples; + unsigned long int d_sample_counter_next; + unsigned int d_pull_in = 0; +}; + +#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc deleted file mode 100644 index 5f9a6ec23..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc +++ /dev/null @@ -1,951 +0,0 @@ -/*! - * \file gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc - * \brief Implementation of a code DLL + carrier PLL tracking block - * \author Marc Majoral, 2017. mmajoral(at)cttc.cat - * Javier Arribas, 2015. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * 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 "gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.h" -#include "gnss_synchro.h" -#include "gps_sdr_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "GPS_L1_CA.h" -#include "gnss_sdr_flags.h" -#include "control_message_factory.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using google::LogMessage; - -gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc_sptr gps_l1_ca_dll_pll_c_aid_make_tracking_fpga_sc( - 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, - int extend_correlation_ms, float early_late_space_chips, - std::string device_name, unsigned int device_base) -{ - return gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc_sptr( - new gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc(if_freq, fs_in, - vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, - pll_bw_narrow_hz, dll_bw_narrow_hz, extend_correlation_ms, - early_late_space_chips, device_name, device_base)); -} - - -void gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::msg_handler_preamble_index( - pmt::pmt_t msg) -{ - DLOG(INFO) << "Extended correlation enabled for Tracking CH " - << d_channel << ": Satellite " - << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); - if (d_enable_extended_integration == false) //avoid re-setting preamble indicator - { - d_preamble_timestamp_s = pmt::to_double(msg); - d_enable_extended_integration = true; - d_preamble_synchronized = false; - } -} - - -gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc( - 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, - int extend_correlation_ms, float early_late_space_chips, - std::string device_name, unsigned int device_base) : gr::block("gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc", - gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), - 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_msg_handler(pmt::mp("preamble_timestamp_s"), - boost::bind( - &gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::msg_handler_preamble_index, - this, _1)); - 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_correlation_length_samples = static_cast(d_vector_length); - - // Initialize tracking ========================================== - d_pll_bw_hz = pll_bw_hz; - d_dll_bw_hz = dll_bw_hz; - d_pll_bw_narrow_hz = pll_bw_narrow_hz; - d_dll_bw_narrow_hz = dll_bw_narrow_hz; - d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); - d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz, 2); - d_extend_correlation_ms = extend_correlation_ms; - - // --- 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(gr_complex), volk_gnsssdr_get_alignment())); - d_ca_code_16sc = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); - - // correlator outputs (scalar) - d_n_correlator_taps = 3; // Early, Prompt, and Late - - d_correlator_outs_16sc = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(lv_16sc_t), - volk_gnsssdr_get_alignment())); - - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs_16sc[n] = lv_cmake(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; - - // create multicorrelator class - multicorrelator_fpga_8sc = std::make_shared(d_n_correlator_taps, device_name, device_base); - - //--- 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_carrier_phase_rad = 0.0; - - // sample synchronization - d_sample_counter = 0; //(from trk to tlm) - 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"); - - set_relative_rate(1.0 / static_cast(d_vector_length)); - - 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_cycles = 0.0; - d_code_phase_samples = 0.0; - d_enable_extended_integration = false; - d_preamble_synchronized = false; - d_rem_code_phase_integer_samples = 0; - d_code_error_chips_Ti = 0.0; - d_pll_to_dll_assist_secs_Ti = 0.0; - d_rem_code_phase_chips = 0.0; - d_code_phase_step_chips = 0.0; - d_carrier_phase_step_rad = 0.0; - d_code_error_filt_chips_s = 0.0; - d_code_error_filt_chips_Ti = 0.0; - d_preamble_timestamp_s = 0.0; - d_carr_phase_error_secs_Ti = 0.0; - //set_min_output_buffer((long int)300); -} - - -void gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::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); - 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.0 / 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_correlation_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(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator - 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_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); - volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS)); - - multicorrelator_fpga_8sc->set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs_16sc[n] = lv_16sc_t(0, 0); - } - - d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0.0; - d_rem_carrier_phase_rad = 0.0; - d_rem_code_phase_chips = 0.0; - d_acc_carrier_phase_cycles = 0.0; - d_pll_to_dll_assist_secs_Ti = 0.0; - 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) << "Tracking of GPS L1 C/A signal for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking - d_pull_in = true; - d_enable_tracking = true; - d_enable_extended_integration = false; - d_preamble_synchronized = false; - - // lock the channel - multicorrelator_fpga_8sc->lock_channel(); - - 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; -} - - -gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::~gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc() -{ - 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_c_aid_tracking_fpga_sc::save_matfile(); - if (d_channel == 0) - { - std::cout << " done." << std::endl; - } - } - - try - { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_ca_code); - volk_gnsssdr_free(d_ca_code_16sc); - volk_gnsssdr_free(d_correlator_outs_16sc); - delete[] d_Prompt_buffer; - multicorrelator_fpga_8sc->free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -void gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::set_channel(unsigned int channel) -{ - d_channel = channel; - multicorrelator_fpga_8sc->set_channel(d_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(); - } - } - } -} - - -int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::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; -} - - -void gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::set_gnss_synchro( - Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} - - -void gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::reset(void) -{ - multicorrelator_fpga_8sc->unlock_channel(); -} - - -int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::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) -{ - // samples offset - int samples_offset; - - // Block input data and block output stream pointers - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - // process vars - double code_error_filt_secs_Ti = 0.0; - double CURRENT_INTEGRATION_TIME_S = 0.0; - double CORRECTED_INTEGRATION_TIME_S = 0.0; - - if (d_enable_tracking == true) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Receiver signal alignment - if (d_pull_in == true) - { - 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_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_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 += samples_offset; // count for the processed samples - d_pull_in = false; - d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - //consume_each(samples_offset); // shift input to perform alignment with local replica - multicorrelator_fpga_8sc->set_initial_sample(samples_offset); - - return 1; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_fpga_8sc->set_output_vectors(d_correlator_outs_16sc); - - multicorrelator_fpga_8sc->Carrier_wipeoff_multicorrelator_resampler( - d_rem_carrier_phase_rad, d_carrier_phase_step_rad, - d_rem_code_phase_chips, d_code_phase_step_chips, - d_correlation_length_samples); - - // ####### coherent integration extension - // keep the last symbols - d_E_history.push_back(d_correlator_outs_16sc[0]); // save early output - d_P_history.push_back(d_correlator_outs_16sc[1]); // save prompt output - d_L_history.push_back(d_correlator_outs_16sc[2]); // save late output - - if (static_cast(d_P_history.size()) > d_extend_correlation_ms) - { - d_E_history.pop_front(); - d_P_history.pop_front(); - d_L_history.pop_front(); - } - - bool enable_dll_pll; - if (d_enable_extended_integration == true) - { - long int symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); - if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) - { - // compute coherent integration and enable tracking loop - // perform coherent integration using correlator output history - // std::cout<<"##### RESET COHERENT INTEGRATION ####"<PRN) - << " pll_bw = " << d_pll_bw_hz - << " [Hz], pll_narrow_bw = " - << d_pll_bw_narrow_hz << " [Hz]" - << std::endl - << " dll_bw = " - << d_dll_bw_hz - << " [Hz], dll_narrow_bw = " - << d_dll_bw_narrow_hz << " [Hz]" - << std::endl; - } - // UPDATE INTEGRATION TIME - CURRENT_INTEGRATION_TIME_S = static_cast(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD; - enable_dll_pll = true; - } - else - { - if (d_preamble_synchronized == true) - { - // continue extended coherent correlation - // Compute the next buffer length based on the 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 * GPS_L1_CA_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - int K_prn_samples = round(T_prn_samples); - double K_T_prn_error_samples = K_prn_samples - T_prn_samples; - - d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples; - d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples - d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; - d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; - // 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_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); - d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast(d_correlation_length_samples), GPS_TWO_PI); - - // UPDATE ACCUMULATED CARRIER PHASE - CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); - d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI; - - // disable tracking loop and inform telemetry decoder - enable_dll_pll = false; - } - else - { - // perform basic (1ms) correlation - // UPDATE INTEGRATION TIME - CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); - enable_dll_pll = true; - } - } - } - else - { - // UPDATE INTEGRATION TIME - CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); - enable_dll_pll = true; - } - - if (enable_dll_pll == true) - { - // ################## PLL ########################################################## - // Update PLL discriminator [rads/Ti -> Secs/Ti] - d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / GPS_TWO_PI; //prompt output - - // Carrier discriminator filter - // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan - // Input [s/Ti] -> output [Hz] - d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S); - // PLL to DLL assistance [Secs/Ti] - d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ; - // code Doppler frequency update - 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 - d_code_error_chips_Ti = dll_nc_e_minus_l_normalized( - std::complex( - d_correlator_outs_16sc[0].real(), - d_correlator_outs_16sc[0].imag()), - std::complex( - d_correlator_outs_16sc[2].real(), - d_correlator_outs_16sc[2].imag())); // [chips/Ti] //early and late - // Code discriminator filter - d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] - d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; - code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti] - - // ################## 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 * GPS_L1_CA_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_prn_samples = round(T_prn_samples); - double K_T_prn_error_samples = K_prn_samples - T_prn_samples; - - d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast(d_fs_in); - d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples - d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; - d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_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); - d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI; - // UPDATE ACCUMULATED CARRIER PHASE - CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); - //remnant carrier phase [rad] - d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI); - - //################### 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_chips = d_rem_code_phase_samples * (d_code_freq_chips / 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] = lv_cmake(static_cast(d_correlator_outs_16sc[1].real()), - static_cast(d_correlator_outs_16sc[1].imag())); // 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, GPS_L1_CA_CODE_PERIOD); - // 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 - multicorrelator_fpga_8sc->unlock_channel(); - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; - 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; - if (d_preamble_synchronized == true) - { - current_synchro_data.correlation_length_ms = d_extend_correlation_ms; - } - else - { - current_synchro_data.correlation_length_ms = 1; - } - } - else - { - current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; // todo: project the carrier doppler - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - } - } - else - { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs_16sc[n] = lv_cmake(0, 0); - } - - current_synchro_data.System = {'G'}; - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; - } - - 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; - float tmp_VE = 0.0; - float tmp_VL = 0.0; - float tmp_float; - prompt_I = d_correlator_outs_16sc[1].real(); - prompt_Q = d_correlator_outs_16sc[1].imag(); - tmp_E = std::abs(gr_complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); - tmp_P = std::abs(gr_complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); - tmp_L = std::abs(gr_complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); - 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_cycles * GPS_TWO_PI; - 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // DLL commands - tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_error_filt_chips_Ti; - 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_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - double tmp_double = static_cast(d_sample_counter + d_correlation_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(); - } - } - - //consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_correlation_length_samples; //count for the processed samples - - if (d_enable_tracking) - { - return 1; - } - else - { - return 0; - } -} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc deleted file mode 100644 index 30a99b5d5..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.cc +++ /dev/null @@ -1,540 +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_fpga_sc.h" -#include "control_message_factory.h" -#include "gnss_sdr_flags.h" -#include "GPS_L1_CA.h" -#include "gps_sdr_signal_processing.h" -#include "lock_detectors.h" -#include "tracking_discriminators.h" -#include -#include -#include -#include -#include -#include -#include - - -using google::LogMessage; - -gps_l1_ca_dll_pll_tracking_fpga_sc_sptr -gps_l1_ca_dll_pll_make_tracking_fpga_sc( - 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, - std::string device_name, - unsigned int device_base) -{ - return gps_l1_ca_dll_pll_tracking_fpga_sc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, device_name, device_base)); -} - - -Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc( - 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, - std::string device_name, - unsigned int device_base) : gr::block("Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc", gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) -{ - // Telemetry bit synchronization message port input - 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); - d_correlation_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())); - //d_ca_code_16sc = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); - //d_ca_code_16sc = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(int), 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; - - // create multicorrelator class - multicorrelator_fpga_8sc = std::make_shared(d_n_correlator_taps, device_name, device_base); - - //--- 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_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)); - - multicorrelator_fpga_8sc->set_output_vectors(d_correlator_outs); -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::start_tracking() -{ - /* - * correct the code phase according to the delay between acq and trk - */ - //printf("TRK : start tracking for satellite %d\n", d_acquisition_gnss_synchro->PRN); - 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); - //gps_l1_ca_code_gen_int(d_ca_code_16sc, d_acquisition_gnss_synchro->PRN, 0); - /* for (int n = 0; n < static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); n++) - { - d_ca_code_16sc[n] = d_ca_code[n]; - } */ - //multicorrelator_fpga_8sc->set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips, d_acquisition_gnss_synchro->PRN); - multicorrelator_fpga_8sc->set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_local_code_shift_chips, d_acquisition_gnss_synchro->PRN); - 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); - 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; //do it in the end to avoid starting running tracking before finishing this function - 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; -} - - -Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::~Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc() -{ - if (d_dump_file.is_open()) - { - try - { - d_dump_file.close(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } - } - try - { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - delete[] d_Prompt_buffer; - multicorrelator_fpga_8sc->free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -int Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::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) -{ - unsigned absolute_samples_offset; - // 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; - - int next_prn_length_samples = d_current_prn_length_samples; - - // Block input data and block output stream pointers - 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) - { - d_pull_in = false; - multicorrelator_fpga_8sc->lock_channel(); - unsigned counter_value = multicorrelator_fpga_8sc->read_sample_counter(); - unsigned num_frames = ceil((counter_value - current_synchro_data.Acq_samplestamp_samples - current_synchro_data.Acq_delay_samples) / d_correlation_length_samples); - absolute_samples_offset = current_synchro_data.Acq_delay_samples + current_synchro_data.Acq_samplestamp_samples + num_frames * d_correlation_length_samples; - multicorrelator_fpga_8sc->set_initial_sample(absolute_samples_offset); - d_sample_counter = absolute_samples_offset; - current_synchro_data.Tracking_sample_counter = absolute_samples_offset; - } - else - { - // continue as from the previous point - d_sample_counter = d_sample_counter_next; - } - d_sample_counter_next = d_sample_counter + d_current_prn_length_samples; - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_fpga_8sc->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] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - next_prn_length_samples = round(K_blk_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 - next_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, GPS_L1_CA_CODE_PERIOD); - // 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 - multicorrelator_fpga_8sc->unlock_channel(); - } - } - - // ########### 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_enable_tracking == true) // in the FPGA case dump data only when tracking is enabled, otherwise the dumped data is useless - { - 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(); - } - } - } - - - d_current_prn_length_samples = next_prn_length_samples; - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - - if (d_enable_tracking == true) - { - return 1; - } - else - { - return 0; - } -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::set_channel(unsigned int channel) -{ - d_channel = channel; - multicorrelator_fpga_8sc->set_channel(d_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_fpga_sc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc::reset(void) -{ - multicorrelator_fpga_8sc->unlock_channel(); -} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h deleted file mode 100644 index 295c3f177..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_fpga_sc.h +++ /dev/null @@ -1,188 +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_FPGA_SC_H -#define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_SC_H - - -#include "gps_sdr_signal_processing.h" -#include "gnss_synchro.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" -#include "fpga_multicorrelator_8sc.h" -#include -#include -#include -#include -#include -#include -#include - - -class Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc; - -typedef boost::shared_ptr - gps_l1_ca_dll_pll_tracking_fpga_sc_sptr; - -gps_l1_ca_dll_pll_tracking_fpga_sc_sptr -gps_l1_ca_dll_pll_make_tracking_fpga_sc(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, - std::string device_name, - unsigned int device_base); - - -/*! - * \brief This class implements a DLL + PLL tracking loop block - */ -class Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc : public gr::block -{ -public: - ~Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc(); - - 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 reset(void); - -private: - friend gps_l1_ca_dll_pll_tracking_fpga_sc_sptr - gps_l1_ca_dll_pll_make_tracking_fpga_sc(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, - std::string device_name, - unsigned int device_base); - - Gps_L1_Ca_Dll_Pll_Tracking_fpga_sc(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, - std::string device_name, - unsigned int device_base); - - // 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; - //int* d_ca_code_16sc; - - float* d_local_code_shift_chips; - gr_complex* d_correlator_outs; - std::shared_ptr multicorrelator_fpga_8sc; - - // 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; - - // extra - int d_correlation_length_samples; - unsigned long int d_sample_counter_next; - double d_rem_carrier_phase_rad; - - double d_K_blk_samples_previous; - int d_offset_sample_previous; -}; - -#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_FPGA_SC_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 88c4eb2a5..4e2a93efd 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -46,7 +46,7 @@ set(TRACKING_LIB_SOURCES ) if(ENABLE_FPGA) - SET(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator_8sc.cc) + SET(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator.cc) endif(ENABLE_FPGA) include_directories( diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc similarity index 71% rename from src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc rename to src/algorithms/tracking/libs/fpga_multicorrelator.cc index 737e414d9..281bf2bc2 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -34,8 +34,10 @@ * ------------------------------------------------------------------------- */ -#include "fpga_multicorrelator_8sc.h" +#include "fpga_multicorrelator.h" + #include + // FPGA stuff #include @@ -65,7 +67,7 @@ // constants #include "GPS_L1_CA.h" -#include "gps_sdr_signal_processing.h" +//#include "gps_sdr_signal_processing.h" #define NUM_PRNs 32 #define PAGE_SIZE 0x10000 @@ -73,7 +75,7 @@ #define CODE_RESAMPLER_NUM_BITS_PRECISION 20 #define CODE_PHASE_STEP_CHIPS_NUM_NBITS CODE_RESAMPLER_NUM_BITS_PRECISION #define pwrtwo(x) (1 << (x)) -#define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS +#define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS #define PHASE_CARR_NBITS 32 #define PHASE_CARR_NBITS_INT 1 #define PHASE_CARR_NBITS_FRAC PHASE_CARR_NBITS - PHASE_CARR_NBITS_INT @@ -84,7 +86,7 @@ int fpga_multicorrelator_8sc::read_sample_counter() { - return d_map_base[7]; + return d_map_base[7]; } void fpga_multicorrelator_8sc::set_initial_sample(int samples_offset) @@ -92,16 +94,17 @@ void fpga_multicorrelator_8sc::set_initial_sample(int samples_offset) d_initial_sample_counter = samples_offset; d_map_base[13] = d_initial_sample_counter; } - + void fpga_multicorrelator_8sc::set_local_code_and_taps(int code_length_chips, - float *shifts_chips, int PRN) + float *shifts_chips, int PRN) { + d_shifts_chips = shifts_chips; d_code_length_chips = code_length_chips; fpga_multicorrelator_8sc::fpga_configure_tracking_gps_local_code(PRN); } -void fpga_multicorrelator_8sc::set_output_vectors(gr_complex *corr_out) +void fpga_multicorrelator_8sc::set_output_vectors(gr_complex* corr_out) { d_corr_out = corr_out; } @@ -113,18 +116,21 @@ void fpga_multicorrelator_8sc::update_local_code(float rem_code_phase_chips) fpga_multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(); } + void fpga_multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( - float rem_carrier_phase_in_rad, float phase_step_rad, - float rem_code_phase_chips, float code_phase_step_chips, - int signal_length_samples) + float rem_carrier_phase_in_rad, float phase_step_rad, + float rem_code_phase_chips, float code_phase_step_chips, + int signal_length_samples) { + + update_local_code(rem_code_phase_chips); d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad; d_code_phase_step_chips = code_phase_step_chips; d_phase_step_rad = phase_step_rad; d_correlator_length_samples = signal_length_samples; - fpga_multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); - fpga_multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); + fpga_multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); + fpga_multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); fpga_multicorrelator_8sc::fpga_launch_multicorrelator_fpga(); int irq_count; ssize_t nb; @@ -137,9 +143,8 @@ void fpga_multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( fpga_multicorrelator_8sc::read_tracking_gps_results(); } - fpga_multicorrelator_8sc::fpga_multicorrelator_8sc(int n_correlators, - std::string device_name, unsigned int device_base) + std::string device_name, unsigned int device_base, int *ca_codes, unsigned int code_length) { d_n_correlators = n_correlators; d_device_name = device_name; @@ -148,10 +153,10 @@ fpga_multicorrelator_8sc::fpga_multicorrelator_8sc(int n_correlators, d_map_base = nullptr; // instantiate variable length vectors - d_initial_index = static_cast(volk_gnsssdr_malloc( - n_correlators * sizeof(unsigned), volk_gnsssdr_get_alignment())); - d_initial_interp_counter = static_cast(volk_gnsssdr_malloc( - n_correlators * sizeof(unsigned), volk_gnsssdr_get_alignment())); + d_initial_index = static_cast(volk_gnsssdr_malloc( + n_correlators * sizeof(unsigned), volk_gnsssdr_get_alignment())); + d_initial_interp_counter = static_cast(volk_gnsssdr_malloc( + n_correlators * sizeof(unsigned), volk_gnsssdr_get_alignment())); //d_local_code_in = nullptr; d_shifts_chips = nullptr; @@ -165,21 +170,24 @@ fpga_multicorrelator_8sc::fpga_multicorrelator_8sc(int n_correlators, d_phase_step_rad_int = 0; d_initial_sample_counter = 0; d_channel = 0; - d_correlator_length_samples = 0; - + d_correlator_length_samples = 0, + d_code_length = code_length; + // pre-compute all the codes - d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); - for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) - { - gps_l1_ca_code_gen_int(&d_ca_codes[(int(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); - } +// d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS*NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); +// for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) +// { +// gps_l1_ca_code_gen_int(&d_ca_codes[(int(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); +// } + d_ca_codes = ca_codes; DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; + } fpga_multicorrelator_8sc::~fpga_multicorrelator_8sc() { - delete[] d_ca_codes; + delete[] d_ca_codes; close_device(); } @@ -187,7 +195,7 @@ fpga_multicorrelator_8sc::~fpga_multicorrelator_8sc() bool fpga_multicorrelator_8sc::free() { // unlock the channel - fpga_multicorrelator_8sc::unlock_channel(); + fpga_multicorrelator_8sc::unlock_channel(); // free the FPGA dynamically created variables if (d_initial_index != nullptr) @@ -208,7 +216,7 @@ bool fpga_multicorrelator_8sc::free() void fpga_multicorrelator_8sc::set_channel(unsigned int channel) { - char device_io_name[MAX_LENGTH_DEVICEIO_NAME]; // driver io name + char device_io_name[MAX_LENGTH_DEVICEIO_NAME]; // driver io name d_channel = channel; // open the device corresponding to the assigned channel @@ -223,12 +231,12 @@ void fpga_multicorrelator_8sc::set_channel(unsigned int channel) LOG(WARNING) << "Cannot open deviceio" << device_io_name; } d_map_base = reinterpret_cast(mmap(NULL, PAGE_SIZE, - PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0)); + PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0)); - if (d_map_base == reinterpret_cast(-1)) + if (d_map_base == reinterpret_cast(-1)) { LOG(WARNING) << "Cannot map the FPGA tracking module " - << d_channel << "into user memory"; + << d_channel << "into user memory"; } // sanity check : check test register @@ -247,7 +255,7 @@ void fpga_multicorrelator_8sc::set_channel(unsigned int channel) unsigned fpga_multicorrelator_8sc::fpga_acquisition_test_register( - unsigned writeval) + unsigned writeval) { unsigned readval; // write value to test register @@ -272,7 +280,7 @@ void fpga_multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int PRN) for (k = 0; k < d_code_length_chips; k++) { //if (d_local_code_in[k] == 1) - if (d_ca_codes[((int(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)) + k] == 1) + if (d_ca_codes[((int(d_code_length)) * (PRN - 1)) + k] == 1) { code_chip = 1; } @@ -281,9 +289,11 @@ void fpga_multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int PRN) code_chip = 0; } // copy the local code to the FPGA memory one by one - d_map_base[11] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | code_chip | select_fpga_correlator; + d_map_base[11] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY + | code_chip | select_fpga_correlator; } - select_fpga_correlator = select_fpga_correlator + LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; + select_fpga_correlator = select_fpga_correlator + + LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; } } @@ -296,20 +306,20 @@ void fpga_multicorrelator_8sc::fpga_compute_code_shift_parameters(void) for (i = 0; i < d_n_correlators; i++) { temp_calculation = floor( - d_shifts_chips[i] - d_rem_code_phase_chips); - + d_shifts_chips[i] - d_rem_code_phase_chips); + if (temp_calculation < 0) { - temp_calculation = temp_calculation + d_code_length_chips; // % operator does not work as in Matlab with negative numbers + temp_calculation = temp_calculation + d_code_length_chips; // % operator does not work as in Matlab with negative numbers } - d_initial_index[i] = static_cast((static_cast(temp_calculation)) % d_code_length_chips); + d_initial_index[i] = static_cast( (static_cast(temp_calculation)) % d_code_length_chips); temp_calculation = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, - 1.0); + 1.0); if (temp_calculation < 0) { - temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers + temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers } - d_initial_interp_counter[i] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * temp_calculation)); + d_initial_interp_counter[i] = static_cast( floor( MAX_CODE_RESAMPLER_COUNTER * temp_calculation)); } } @@ -322,7 +332,7 @@ void fpga_multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void) d_map_base[1 + i] = d_initial_index[i]; d_map_base[1 + d_n_correlators + i] = d_initial_interp_counter[i]; } - d_map_base[8] = d_code_length_chips - 1; // number of samples - 1 + d_map_base[8] = d_code_length_chips - 1; // number of samples - 1 } @@ -330,27 +340,30 @@ void fpga_multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) { float d_rem_carrier_phase_in_rad_temp; - d_code_phase_step_chips_num = static_cast(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips)); + d_code_phase_step_chips_num = static_cast( roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips)); if (d_rem_carrier_phase_in_rad > M_PI) { - d_rem_carrier_phase_in_rad_temp = -2 * M_PI + d_rem_carrier_phase_in_rad; + d_rem_carrier_phase_in_rad_temp = -2 * M_PI + + d_rem_carrier_phase_in_rad; } else if (d_rem_carrier_phase_in_rad < -M_PI) { - d_rem_carrier_phase_in_rad_temp = 2 * M_PI + d_rem_carrier_phase_in_rad; + d_rem_carrier_phase_in_rad_temp = 2 * M_PI + + d_rem_carrier_phase_in_rad; } else { d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad; } - d_rem_carr_phase_rad_int = static_cast(roundf( - (fabs(d_rem_carrier_phase_in_rad_temp) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); + d_rem_carr_phase_rad_int = static_cast( roundf( + (fabs(d_rem_carrier_phase_in_rad_temp) / M_PI) + * pow(2, PHASE_CARR_NBITS_FRAC))); if (d_rem_carrier_phase_in_rad_temp < 0) { d_rem_carr_phase_rad_int = -d_rem_carr_phase_rad_int; } - d_phase_step_rad_int = static_cast(roundf( - (fabs(d_phase_step_rad) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); // the FPGA accepts a range for the phase step between -pi and +pi + d_phase_step_rad_int = static_cast( roundf( + (fabs(d_phase_step_rad) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); // the FPGA accepts a range for the phase step between -pi and +pi if (d_phase_step_rad < 0) { @@ -372,10 +385,10 @@ void fpga_multicorrelator_8sc::fpga_launch_multicorrelator_fpga(void) { // enable interrupts int reenable = 1; - write(d_device_descriptor, reinterpret_cast(&reenable), sizeof(int)); + write(d_device_descriptor, reinterpret_cast(&reenable), sizeof(int)); - // writing 1 to reg 14 launches the tracking - d_map_base[14] = 1; + // writing 1 to reg 14 launches the tracking + d_map_base[14] = 1; } @@ -388,17 +401,17 @@ void fpga_multicorrelator_8sc::read_tracking_gps_results(void) for (k = 0; k < d_n_correlators; k++) { readval_real = d_map_base[1 + k]; - if (readval_real >= 1048576) // 0x100000 (21 bits two's complement) + if (readval_real >= 1048576) // 0x100000 (21 bits two's complement) { readval_real = -2097152 + readval_real; } readval_imag = d_map_base[1 + d_n_correlators + k]; - if (readval_imag >= 1048576) // 0x100000 (21 bits two's complement) + if (readval_imag >= 1048576) // 0x100000 (21 bits two's complement) { readval_imag = -2097152 + readval_imag; } - d_corr_out[k] = gr_complex(readval_real, readval_imag); + d_corr_out[k] = gr_complex(readval_real,readval_imag); } } @@ -406,42 +419,40 @@ void fpga_multicorrelator_8sc::read_tracking_gps_results(void) void fpga_multicorrelator_8sc::unlock_channel(void) { // unlock the channel to let the next samples go through - d_map_base[12] = 1; // unlock the channel + d_map_base[12] = 1; // unlock the channel } - void fpga_multicorrelator_8sc::close_device() { - unsigned *aux = const_cast(d_map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + unsigned * aux = const_cast(d_map_base); + if (munmap(static_cast(aux), PAGE_SIZE) == -1) { printf("Failed to unmap memory uio\n"); } - /* else +/* else { printf("memory uio unmapped\n"); } */ close(d_device_descriptor); } - + void fpga_multicorrelator_8sc::lock_channel(void) { // lock the channel for processing - d_map_base[12] = 0; // lock the channel + d_map_base[12] = 0; // lock the channel } - void fpga_multicorrelator_8sc::read_sample_counters(int *sample_counter, int *secondary_sample_counter, int *counter_corr_0_in, int *counter_corr_0_out) { - *sample_counter = d_map_base[11]; - *secondary_sample_counter = d_map_base[8]; - *counter_corr_0_in = d_map_base[10]; - *counter_corr_0_out = d_map_base[9]; + *sample_counter = d_map_base[11]; + *secondary_sample_counter = d_map_base[8]; + *counter_corr_0_in = d_map_base[10]; + *counter_corr_0_out = d_map_base[9]; + } - void fpga_multicorrelator_8sc::reset_multicorrelator(void) { - d_map_base[14] = 2; // writing a 2 to d_map_base[14] resets the multicorrelator + d_map_base[14] = 2; // writing a 2 to d_map_base[14] resets the multicorrelator } diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h similarity index 78% rename from src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h rename to src/algorithms/tracking/libs/fpga_multicorrelator.h index 1eceb1936..5012651d4 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator_8sc.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -49,46 +49,46 @@ class fpga_multicorrelator_8sc { public: fpga_multicorrelator_8sc(int n_correlators, std::string device_name, - unsigned int device_base); + unsigned int device_base, int *ca_codes, unsigned int code_length); ~fpga_multicorrelator_8sc(); - //bool set_output_vectors(gr_complex* corr_out); - void set_output_vectors(gr_complex *corr_out); - // bool set_local_code_and_taps( - // int code_length_chips, const int* local_code_in, - // float *shifts_chips, int PRN); + //bool set_output_vectors(gr_complex* corr_out); + void set_output_vectors(gr_complex* corr_out); +// bool set_local_code_and_taps( +// int code_length_chips, const int* local_code_in, +// float *shifts_chips, int PRN); //bool set_local_code_and_taps( void set_local_code_and_taps( - int code_length_chips, - float *shifts_chips, int PRN); + int code_length_chips, + float *shifts_chips, int PRN); //bool set_output_vectors(lv_16sc_t* corr_out); void update_local_code(float rem_code_phase_chips); //bool Carrier_wipeoff_multicorrelator_resampler( void Carrier_wipeoff_multicorrelator_resampler( - float rem_carrier_phase_in_rad, float phase_step_rad, - float rem_code_phase_chips, float code_phase_step_chips, - int signal_length_samples); - bool free(); + float rem_carrier_phase_in_rad, float phase_step_rad, + float rem_code_phase_chips, float code_phase_step_chips, + int signal_length_samples);bool free(); void set_channel(unsigned int channel); void set_initial_sample(int samples_offset); int read_sample_counter(); void lock_channel(void); void unlock_channel(void); - void read_sample_counters(int *sample_counter, int *secondary_sample_counter, int *counter_corr_0_in, int *counter_corr_0_out); // debug - + void read_sample_counters(int *sample_counter, int *secondary_sample_counter, int *counter_corr_0_in, int *counter_corr_0_out); // debug + + private: //const int *d_local_code_in; - gr_complex *d_corr_out; + gr_complex * d_corr_out; float *d_shifts_chips; int d_code_length_chips; int d_n_correlators; // data related to the hardware module and the driver - int d_device_descriptor; // driver descriptor - volatile unsigned *d_map_base; // driver memory map + int d_device_descriptor; // driver descriptor + volatile unsigned *d_map_base; // driver memory map // configuration data received from the interface - unsigned int d_channel; // channel number - unsigned d_ncorrelators; // number of correlators + unsigned int d_channel; // channel number + unsigned d_ncorrelators; // number of correlators unsigned d_correlator_length_samples; float d_rem_code_phase_chips; float d_code_phase_step_chips; @@ -107,7 +107,10 @@ private: std::string d_device_name; unsigned int d_device_base; - int *d_ca_codes; + + int* d_ca_codes; + + unsigned int d_code_length; // nominal number of chips // private functions unsigned fpga_acquisition_test_register(unsigned writeval); @@ -118,8 +121,11 @@ private: void fpga_configure_signal_parameters_in_fpga(void); void fpga_launch_multicorrelator_fpga(void); void read_tracking_gps_results(void); - void reset_multicorrelator(void); - void close_device(void); + void reset_multicorrelator(void); + void close_device(void); + + // debug + //unsigned int first_time = 1; }; #endif /* GNSS_SDR_FPGA_MULTICORRELATOR_H_ */ diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc index 797f2c3d9..896c2a6c0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc @@ -36,8 +36,8 @@ #include #include #include -#include // to test the FPGA we have to create a simultaneous task to send the samples using the DMA and stop the test -#include // FPGA read input file +#include // to test the FPGA we have to create a simultaneous task to send the samples using the DMA and stop the test +#include // FPGA read input file #include #include #include @@ -61,17 +61,17 @@ #include "signal_generator_flags.h" #include "interleaved_byte_to_complex_short.h" -#define DMA_TRACK_TRANSFER_SIZE 2046 // DMA transfer size for tracking -#define MIN_SAMPLES_REMAINING 20000 // number of remaining samples in the DMA that causes the CPU to stop the flowgraph (it has to be a bit alrger than 2x max packet size) -#define FIVE_SECONDS 5000000 // five seconds in microseconds +#define DMA_TRACK_TRANSFER_SIZE 2046 // DMA transfer size for tracking +#define MIN_SAMPLES_REMAINING 20000 // number of remaining samples in the DMA that causes the CPU to stop the flowgraph (it has to be a bit alrger than 2x max packet size) +#define FIVE_SECONDS 5000000 // five seconds in microseconds void send_tracking_gps_input_samples(FILE *rx_signal_file, - int num_remaining_samples, gr::top_block_sptr top_block) + int num_remaining_samples, gr::top_block_sptr top_block) { - int num_samples_transferred = 0; // number of samples that have been transferred to the DMA so far - static int flowgraph_stopped = 0; // flag to indicate if the flowgraph is stopped already - char *buffer_DMA; // temporary buffer to store the samples to be sent to the DMA - int dma_descr; // DMA descriptor + int num_samples_transferred = 0; // number of samples that have been transferred to the DMA so far + static int flowgraph_stopped = 0; // flag to indicate if the flowgraph is stopped already + char *buffer_DMA; // temporary buffer to store the samples to be sent to the DMA + int dma_descr; // DMA descriptor dma_descr = open("/dev/loop_tx", O_WRONLY); if (dma_descr < 0) { @@ -79,7 +79,7 @@ void send_tracking_gps_input_samples(FILE *rx_signal_file, exit(1); } - buffer_DMA = (char *)malloc(DMA_TRACK_TRANSFER_SIZE); + buffer_DMA = (char *) malloc(DMA_TRACK_TRANSFER_SIZE); if (!buffer_DMA) { fprintf(stderr, "Memory error!"); @@ -98,7 +98,8 @@ void send_tracking_gps_input_samples(FILE *rx_signal_file, } if (num_remaining_samples > DMA_TRACK_TRANSFER_SIZE) { - fread(buffer_DMA, DMA_TRACK_TRANSFER_SIZE, 1, rx_signal_file); + + fread(buffer_DMA, DMA_TRACK_TRANSFER_SIZE, 1,rx_signal_file); assert(DMA_TRACK_TRANSFER_SIZE == write(dma_descr, &buffer_DMA[0], DMA_TRACK_TRANSFER_SIZE)); num_remaining_samples = num_remaining_samples - DMA_TRACK_TRANSFER_SIZE; @@ -120,11 +121,11 @@ void send_tracking_gps_input_samples(FILE *rx_signal_file, // thread that sends the samples to the FPGA -void thread(gr::top_block_sptr top_block, const char *file_name) +void thread(gr::top_block_sptr top_block, const char * file_name) { // file descriptor - FILE *rx_signal_file; // file descriptor - int file_length; // length of the file containing the received samples + FILE *rx_signal_file; // file descriptor + int file_length; // length of the file containing the received samples rx_signal_file = fopen(file_name, "rb"); if (!rx_signal_file) @@ -136,7 +137,7 @@ void thread(gr::top_block_sptr top_block, const char *file_name) file_length = ftell(rx_signal_file); fseek(rx_signal_file, 0, SEEK_SET); - usleep(FIVE_SECONDS); // wait for some time to give time to the other thread to program the device + usleep(FIVE_SECONDS); // wait for some time to give time to the other thread to program the device //send_tracking_gps_input_samples(dma_descr, rx_signal_file, file_length); send_tracking_gps_input_samples(rx_signal_file, file_length, top_block); @@ -162,14 +163,14 @@ private: public: int rx_message; - ~GpsL1CADllPllTrackingTestFpga_msg_rx(); //!< Default destructor + ~GpsL1CADllPllTrackingTestFpga_msg_rx(); //!< Default destructor }; GpsL1CADllPllTrackingTestFpga_msg_rx_sptr GpsL1CADllPllTrackingTestFpga_msg_rx_make() { return GpsL1CADllPllTrackingTestFpga_msg_rx_sptr( - new GpsL1CADllPllTrackingTestFpga_msg_rx()); + new GpsL1CADllPllTrackingTestFpga_msg_rx()); } @@ -180,7 +181,7 @@ void GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events(pmt::pmt_t msg) long int message = pmt::to_long(msg); rx_message = message; } - catch (boost::bad_any_cast &e) + catch (boost::bad_any_cast& e) { LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; rx_message = 0; @@ -188,22 +189,22 @@ void GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events(pmt::pmt_t msg) } -GpsL1CADllPllTrackingTestFpga_msg_rx::GpsL1CADllPllTrackingTestFpga_msg_rx() : gr::block("GpsL1CADllPllTrackingTestFpga_msg_rx", - gr::io_signature::make(0, 0, 0), - gr::io_signature::make(0, 0, 0)) +GpsL1CADllPllTrackingTestFpga_msg_rx::GpsL1CADllPllTrackingTestFpga_msg_rx() : + gr::block("GpsL1CADllPllTrackingTestFpga_msg_rx", + gr::io_signature::make(0, 0, 0), + gr::io_signature::make(0, 0, 0)) { this->message_port_register_in(pmt::mp("events")); this->set_msg_handler(pmt::mp("events"), - boost::bind( - &GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events, - this, _1)); + boost::bind( + &GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events, + this, _1)); rx_message = 0; } GpsL1CADllPllTrackingTestFpga_msg_rx::~GpsL1CADllPllTrackingTestFpga_msg_rx() -{ -} +{} // ########################################################### @@ -225,12 +226,12 @@ public: int configure_generator(); int generate_signal(); - void check_results_doppler(arma::vec &true_time_s, arma::vec &true_value, - arma::vec &meas_time_s, arma::vec &meas_value); - void check_results_acc_carrier_phase(arma::vec &true_time_s, - arma::vec &true_value, arma::vec &meas_time_s, arma::vec &meas_value); - void check_results_codephase(arma::vec &true_time_s, arma::vec &true_value, - arma::vec &meas_time_s, arma::vec &meas_value); + void check_results_doppler(arma::vec & true_time_s, arma::vec & true_value, + arma::vec & meas_time_s, arma::vec & meas_value); + void check_results_acc_carrier_phase(arma::vec & true_time_s, + arma::vec & true_value, arma::vec & meas_time_s, arma::vec & meas_value); + void check_results_codephase(arma::vec & true_time_s, arma::vec & true_value, + arma::vec & meas_time_s, arma::vec & meas_value); GpsL1CADllPllTrackingTestFpga() { @@ -262,15 +263,16 @@ int GpsL1CADllPllTrackingTestFpga::configure_generator() p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; if (FLAGS_dynamic_position.empty()) { - p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(FLAGS_duration * 10); + p2 = std::string("-static_position=") + FLAGS_static_position + + std::string(",") + std::to_string(FLAGS_duration * 10); } else { p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position); } - p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output - p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples - p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps] + p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output + p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples + p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps] return 0; } @@ -279,8 +281,8 @@ int GpsL1CADllPllTrackingTestFpga::generate_signal() { int child_status; - char *const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], - &p4[0], &p5[0], NULL}; + char * const parmList[] = { &generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], + &p4[0], &p5[0], NULL }; int pid; if ((pid = fork()) == -1) @@ -308,12 +310,12 @@ void GpsL1CADllPllTrackingTestFpga::configure_receiver() gnss_synchro.PRN = FLAGS_test_satellite_PRN; config->set_property("GNSS-SDR.internal_fs_sps", - std::to_string(baseband_sampling_freq)); + std::to_string(baseband_sampling_freq)); // Set Tracking //config->set_property("Tracking_1C.implementation", // "GPS_L1_CA_DLL_PLL_C_Aid_Tracking_Fpga"); config->set_property("Tracking_1C.implementation", - "GPS_L1_CA_DLL_PLL_Tracking_Fpga"); + "GPS_L1_CA_DLL_PLL_Tracking_Fpga"); config->set_property("Tracking_1C.item_type", "cshort"); config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); @@ -326,8 +328,8 @@ void GpsL1CADllPllTrackingTestFpga::configure_receiver() } -void GpsL1CADllPllTrackingTestFpga::check_results_doppler(arma::vec &true_time_s, - arma::vec &true_value, arma::vec &meas_time_s, arma::vec &meas_value) +void GpsL1CADllPllTrackingTestFpga::check_results_doppler(arma::vec & true_time_s, + arma::vec & true_value, arma::vec & meas_time_s, arma::vec & meas_value) { //1. True value interpolation to match the measurement times arma::vec true_value_interp; @@ -360,13 +362,13 @@ void GpsL1CADllPllTrackingTestFpga::check_results_doppler(arma::vec &true_time_s << ", mean=" << error_mean << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl; - std::cout.precision(ss); + std::cout.precision (ss); } void GpsL1CADllPllTrackingTestFpga::check_results_acc_carrier_phase( - arma::vec &true_time_s, arma::vec &true_value, arma::vec &meas_time_s, - arma::vec &meas_value) + arma::vec & true_time_s, arma::vec & true_value, arma::vec & meas_time_s, + arma::vec & meas_value) { //1. True value interpolation to match the measurement times arma::vec true_value_interp; @@ -399,13 +401,13 @@ void GpsL1CADllPllTrackingTestFpga::check_results_acc_carrier_phase( << ", mean=" << error_mean << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl; - std::cout.precision(ss); + std::cout.precision (ss); } void GpsL1CADllPllTrackingTestFpga::check_results_codephase( - arma::vec &true_time_s, arma::vec &true_value, arma::vec &meas_time_s, - arma::vec &meas_value) + arma::vec & true_time_s, arma::vec & true_value, arma::vec & meas_time_s, + arma::vec & meas_value) { //1. True value interpolation to match the measurement times arma::vec true_value_interp; @@ -437,7 +439,7 @@ void GpsL1CADllPllTrackingTestFpga::check_results_codephase( << ", mean=" << error_mean << ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Chips]" << std::endl; - std::cout.precision(ss); + std::cout.precision (ss); } @@ -461,29 +463,27 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) true_obs_file.append(std::to_string(test_satellite_PRN)); true_obs_file.append(".dat"); ASSERT_NO_THROW( - { - if (true_obs_data.open_obs_file(true_obs_file) == false) { - throw std::exception(); - }; - }) - << "Failure opening true observables file"; + if (true_obs_data.open_obs_file(true_obs_file) == false) + { + throw std::exception(); + }; + }) << "Failure opening true observables file"; top_block = gr::make_top_block("Tracking test"); //std::shared_ptr tracking = std::make_shared (config.get(), "Tracking_1C", 1, 1); - std::shared_ptr tracking = std::make_shared(config.get(), "Tracking_1C", 1, 1); + std::shared_ptr tracking = std::make_shared (config.get(), "Tracking_1C", 1, 1); boost::shared_ptr msg_rx = GpsL1CADllPllTrackingTestFpga_msg_rx_make(); // load acquisition data based on the first epoch of the true observations ASSERT_NO_THROW( - { - if (true_obs_data.read_binary_obs() == false) { - throw std::exception(); - }; - }) - << "Failure reading true observables file"; + if (true_obs_data.read_binary_obs() == false) + { + throw std::exception(); + }; + }) << "Failure reading true observables file"; //restart the epoch counter true_obs_data.restart(); @@ -492,54 +492,52 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) << " Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << std::endl; - gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS + - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) + * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz; gnss_synchro.Acq_samplestamp_samples = 0; ASSERT_NO_THROW( - { - tracking->set_channel(gnss_synchro.Channel_ID); - }) - << "Failure setting channel."; + { + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel."; ASSERT_NO_THROW( - { - tracking->set_gnss_synchro(&gnss_synchro); - }) - << "Failure setting gnss_synchro."; + { + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."; ASSERT_NO_THROW( - { - tracking->connect(top_block); - }) - << "Failure connecting tracking to the top_block."; + { + tracking->connect(top_block); + }) << "Failure connecting tracking to the top_block."; ASSERT_NO_THROW( - { - gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); - top_block->connect(tracking->get_right_block(), 0, sink, 0); - top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); - }) - << "Failure connecting the blocks of tracking test."; + { + gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of tracking test."; tracking->start_tracking(); // assemble again the file name in a null terminated string (not available by default in the main program flow) std::string file = "./" + filename_raw_data; - const char *file_name = file.c_str(); + const char * file_name = file.c_str(); // start thread that sends the DMA samples to the FPGA - boost::thread t{thread, top_block, file_name}; + boost::thread t + { thread, top_block, file_name }; EXPECT_NO_THROW( - { - start = std::chrono::system_clock::now(); - top_block->run(); // Start threads and wait - tracking->reset(); // unlock the channel - end = std::chrono::system_clock::now(); - elapsed_seconds = end - start; - }) - << "Failure running the top_block."; + { + start = std::chrono::system_clock::now(); + top_block->run(); // Start threads and wait + //tracking->reset();// unlock the channel + end = std::chrono::system_clock::now(); + elapsed_seconds = end - start; + }) << "Failure running the top_block."; // wait until child thread terminates t.join(); @@ -569,13 +567,12 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) //load the measured values tracking_dump_reader trk_dump; ASSERT_NO_THROW( - { - if (trk_dump.open_obs_file(std::string("./tracking_ch_0.dat")) == false) { - throw std::exception(); - }; - }) - << "Failure opening tracking dump file"; + if (trk_dump.open_obs_file(std::string("./tracking_ch_0.dat")) == false) + { + throw std::exception(); + }; + }) << "Failure opening tracking dump file"; nepoch = trk_dump.num_epochs(); std::cout << "Measured observation epochs=" << nepoch << std::endl; @@ -588,11 +585,14 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) epoch_counter = 0; while (trk_dump.read_binary_obs()) { - trk_timestamp_s(epoch_counter) = static_cast(trk_dump.PRN_start_sample_count) / static_cast(baseband_sampling_freq); + trk_timestamp_s(epoch_counter) = static_cast(trk_dump.PRN_start_sample_count) + / static_cast(baseband_sampling_freq); trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI; trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz; - double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast(baseband_sampling_freq), 1.0e-3) / 1.0e-3); + double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS + * (fmod( (static_cast(trk_dump.PRN_start_sample_count) + trk_dump.aux1) + / static_cast(baseband_sampling_freq), 1.0e-3) / 1.0e-3); trk_prn_delay_chips(epoch_counter) = delay_chips; epoch_counter++; @@ -600,7 +600,7 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) //Align initial measurements and cut the tracking pull-in transitory double pull_in_offset_s = 1.0; - arma::uvec initial_meas_point = arma::find(trk_timestamp_s >= (true_timestamp_s(0) + pull_in_offset_s), 1, "first"); + arma::uvec initial_meas_point = arma::find( trk_timestamp_s >= (true_timestamp_s(0) + pull_in_offset_s), 1, "first"); trk_timestamp_s = trk_timestamp_s.subvec(initial_meas_point(0), trk_timestamp_s.size() - 1); trk_acc_carrier_phase_cycles = trk_acc_carrier_phase_cycles.subvec(initial_meas_point(0), trk_acc_carrier_phase_cycles.size() - 1); @@ -610,8 +610,8 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga) check_results_doppler(true_timestamp_s, true_Doppler_Hz, trk_timestamp_s, trk_Doppler_Hz); check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips); check_results_acc_carrier_phase(true_timestamp_s, - true_acc_carrier_phase_cycles, trk_timestamp_s, - trk_acc_carrier_phase_cycles); + true_acc_carrier_phase_cycles, trk_timestamp_s, + trk_acc_carrier_phase_cycles); std::cout << "Signal tracking completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; }