diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index f58e1c802..1435379f5 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -49,6 +49,7 @@ endif() set(TRACKING_ADAPTER_SOURCES galileo_e1_dll_pll_veml_tracking.cc + galileo_e1_mixed_veml_tracking.cc galileo_e1_tcp_connector_tracking.cc gps_l1_ca_dll_pll_tracking.cc gps_l1_ca_tcp_connector_tracking.cc @@ -67,6 +68,7 @@ set(TRACKING_ADAPTER_SOURCES set(TRACKING_ADAPTER_HEADERS galileo_e1_dll_pll_veml_tracking.h + galileo_e1_mixed_veml_tracking.h galileo_e1_tcp_connector_tracking.h gps_l1_ca_dll_pll_tracking.h gps_l1_ca_tcp_connector_tracking.h diff --git a/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc new file mode 100644 index 000000000..90858ae02 --- /dev/null +++ b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc @@ -0,0 +1,252 @@ +/*! + * \file galileo_e1_mixed_veml_tracking.cc + * \brief Adapts a DLL+PLL VEML (Very Early Minus Late) tracking loop block + * to a TrackingInterface for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.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-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 "galileo_e1_mixed_veml_tracking.h" +#include "Galileo_E1.h" +#include "configuration_interface.h" +#include "display.h" +#include "dll_pll_conf.h" +#include "gnss_sdr_flags.h" +#include + + +GalileoE1MixedVemlTracking::GalileoE1MixedVemlTracking( + ConfigurationInterface* configuration, const std::string& role, + unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + Dll_Pll_Conf trk_param = Dll_Pll_Conf(); + DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## + std::string default_item_type = "gr_complex"; + std::string item_type = configuration->property(role + ".item_type", default_item_type); + int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + trk_param.fs_in = fs_in; + bool dump = configuration->property(role + ".dump", false); + trk_param.dump = dump; + std::string default_dump_filename = "./track_ch"; + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + trk_param.dump_filename = dump_filename; + bool dump_mat = configuration->property(role + ".dump_mat", true); + trk_param.dump_mat = dump_mat; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: Gal. E1. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); + } + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } + trk_param.pll_bw_hz = pll_bw_hz; + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } + trk_param.dll_bw_hz = dll_bw_hz; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); + + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); + trk_param.early_late_space_chips = early_late_space_chips; + float very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.5); + trk_param.very_early_late_space_chips = very_early_late_space_chips; + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; + float very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.5); + trk_param.very_early_late_space_narrow_chips = very_early_late_space_narrow_chips; + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E1. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (4 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E1. Extended coherent integration is not allowed when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; + } + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: Galileo E1. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } + trk_param.track_pilot = track_pilot; + trk_param.extend_correlation_symbols = extend_correlation_symbols; + int vector_length = std::round(fs_in / (GALILEO_E1_CODE_CHIP_RATE_HZ / GALILEO_E1_B_CODE_LENGTH_CHIPS)); + trk_param.vector_length = vector_length; + trk_param.system = 'E'; + char sig_[3] = "1B"; + std::memcpy(trk_param.signal, sig_, 3); + trk_param.cn0_samples = configuration->property(role + ".cn0_samples", trk_param.cn0_samples); + trk_param.cn0_min = configuration->property(role + ".cn0_min", trk_param.cn0_min); + trk_param.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param.max_code_lock_fail); + trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail); + trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th); + + //################# MAKE TRACKING GNURadio object ################### + if (item_type == "gr_complex") + { + item_size_ = sizeof(gr_complex); + tracking_ = mixed_veml_make_tracking(trk_param); + } + else + { + item_size_ = sizeof(gr_complex); + LOG(WARNING) << item_type << " unknown tracking item type."; + } + + channel_ = 0; + DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; + if (in_streams_ > 1) + { + LOG(ERROR) << "This implementation only supports one input stream"; + } + if (out_streams_ > 1) + { + LOG(ERROR) << "This implementation only supports one output stream"; + } +} + + +GalileoE1MixedVemlTracking::~GalileoE1MixedVemlTracking() = default; + + +void GalileoE1MixedVemlTracking::stop_tracking() +{ +} + + +void GalileoE1MixedVemlTracking::start_tracking() +{ + tracking_->start_tracking(); +} + + +/* + * Set tracking channel unique ID + */ +void GalileoE1MixedVemlTracking::set_channel(unsigned int channel) +{ + channel_ = channel; + tracking_->set_channel(channel); +} + + +void GalileoE1MixedVemlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + tracking_->set_gnss_synchro(p_gnss_synchro); +} + + +void GalileoE1MixedVemlTracking::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 +} + + +void GalileoE1MixedVemlTracking::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 +} + + +gr::basic_block_sptr GalileoE1MixedVemlTracking::get_left_block() +{ + return tracking_; +} + + +gr::basic_block_sptr GalileoE1MixedVemlTracking::get_right_block() +{ + return tracking_; +} diff --git a/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.h b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.h new file mode 100644 index 000000000..2b50d3ae0 --- /dev/null +++ b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.h @@ -0,0 +1,109 @@ +/*! + * \file galileo_e1_mixed_veml_tracking.h + * \brief Adapts a DLL+PLL VEML (Very Early Minus Late) tracking loop block + * to a TrackingInterface for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.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-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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GALILEO_E1_MIXED_VEML_TRACKING_H_ +#define GNSS_SDR_GALILEO_E1_MIXED_VEML_TRACKING_H_ + +#include "mixed_veml_tracking.h" +#include "tracking_interface.h" +#include + + +class ConfigurationInterface; + +/*! + * \brief This class Adapts a DLL+PLL VEML (Very Early Minus Late) tracking + * loop block to a TrackingInterface for Galileo E1 signals + */ +class GalileoE1MixedVemlTracking : public TrackingInterface +{ +public: + GalileoE1MixedVemlTracking(ConfigurationInterface* configuration, + const std::string& role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GalileoE1MixedVemlTracking(); + + inline std::string role() override + { + return role_; + } + + //! Returns "Galileo_E1_MIXED_VEML_Tracking" + inline std::string implementation() override + { + return "Galileo_E1_Mixed_VEML_Tracking"; + } + + inline size_t item_size() override + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; + + /*! + * \brief Set tracking channel unique ID + */ + void set_channel(unsigned int channel) override; + + /*! + * \brief Set acquisition/tracking common Gnss_Synchro object pointer + * to efficiently exchange synchronization data between acquisition and + * tracking blocks + */ + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + + void start_tracking() override; + /*! + * \brief Stop running tracking + */ + void stop_tracking() override; + +private: + mixed_veml_tracking_sptr tracking_; + size_t item_size_; + unsigned int channel_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; +}; + +#endif // GNSS_SDR_GALILEO_E1_MIXED_VEML_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 77c9a01f5..9ac5537e1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -49,6 +49,7 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc dll_pll_veml_tracking.cc + mixed_veml_tracking.cc ${OPT_TRACKING_BLOCKS_SOURCES} ) @@ -62,6 +63,7 @@ set(TRACKING_GR_BLOCKS_HEADERS glonass_l2_ca_dll_pll_tracking_cc.h glonass_l2_ca_dll_pll_c_aid_tracking_cc.h glonass_l2_ca_dll_pll_c_aid_tracking_sc.h + mixed_veml_tracking.h dll_pll_veml_tracking.h ${OPT_TRACKING_BLOCKS_HEADERS} ) diff --git a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc new file mode 100644 index 000000000..63a562e2b --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc @@ -0,0 +1,1881 @@ +/*! + * \file mixed_veml_tracking.cc + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author 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 "mixed_veml_tracking.h" +#include "Beidou_B1I.h" +#include "Beidou_B3I.h" +#include "GPS_L1_CA.h" +#include "GPS_L2C.h" +#include "GPS_L5.h" +#include "Galileo_E1.h" +#include "Galileo_E5a.h" +#include "MATH_CONSTANTS.h" +#include "beidou_b1i_signal_processing.h" +#include "beidou_b3i_signal_processing.h" +#include "galileo_e1_signal_processing.h" +#include "galileo_e5_signal_processing.h" +#include "gnss_sdr_create_directory.h" +#include "gnss_synchro.h" +#include "gps_l2c_signal.h" +#include "gps_l5_signal.h" +#include "gps_sdr_signal_processing.h" +#include "lock_detectors.h" +#include "tracking_discriminators.h" +#include +#include // for io_signature +#include // for scoped_lock +#include // for Mat_VarCreate +#include // for mp +#include +#include // for fill_n +#include // for fmod, round, floor +#include // for exception +#include +#include // for cout, cerr +#include +#include +#include + +#if HAS_STD_FILESYSTEM +#if HAS_STD_FILESYSTEM_EXPERIMENTAL +#include +namespace fs = std::experimental::filesystem; +#else +#include +namespace fs = std::filesystem; +#endif +#else +#include +namespace fs = boost::filesystem; +#endif + + +mixed_veml_tracking_sptr mixed_veml_make_tracking(const Dll_Pll_Conf &conf_) +{ + return mixed_veml_tracking_sptr(new mixed_veml_tracking(conf_)); +} + + +mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("mixed_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); + 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)); + + // Telemetry message port input + this->message_port_register_in(pmt::mp("telemetry_to_trk")); + this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&mixed_veml_tracking::msg_handler_telemetry_to_trk, this, _1)); + + // initialize internal vars + d_dll_filt_history.set_capacity(1000); + d_veml = false; + d_cloop = true; + d_pull_in_transitory = true; + d_code_chip_rate = 0.0; + d_secondary_code_length = 0U; + d_secondary_code_string = nullptr; + d_data_secondary_code_length = 0U; + d_data_secondary_code_string = nullptr; + d_preambles_symbols = nullptr; + d_preamble_length_symbols = 0; + 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"; + map_signal_pretty_name["B1"] = "B1I"; + map_signal_pretty_name["B3"] = "B3I"; + + signal_pretty_name = map_signal_pretty_name[signal_type]; + + if (trk_parameters.system == 'G') + { + systemName = "GPS"; + if (signal_type == "1C") + { + 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_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; + // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit + // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization + d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + } + else if (signal_type == "2S") + { + 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); + // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required + 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; + } + else if (signal_type == "L5") + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5I_PERIOD; + d_code_chip_rate = GPS_L5I_CODE_RATE_HZ; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit + 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) + { + // synchronize pilot secondary code + d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); + signal_pretty_name = signal_pretty_name + "Q"; + } + else + { + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + 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"; + } + } + 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; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0U; + d_code_samples_per_chip = 0U; + d_symbols_per_bit = 0; + } + } + else if (trk_parameters.system == 'E') + { + systemName = "Galileo"; + if (signal_type == "1B") + { + 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); + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required + 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"; + } + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + } + else if (signal_type == "5X") + { + 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) + { + // synchronize pilot secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); + signal_pretty_name = signal_pretty_name + "Q"; + // remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + } + else + { + // synchronize and remove data secondary code + 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"; + } + } + 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; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0U; + d_code_samples_per_chip = 0U; + d_symbols_per_bit = 0; + } + } + else if (trk_parameters.system == 'C') + { + systemName = "Beidou"; + if (signal_type == "B1") + { + // GEO Satellites use different secondary code + d_signal_carrier_freq = BEIDOU_B1I_FREQ_HZ; + d_code_period = BEIDOU_B1I_CODE_PERIOD; + d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ; + d_code_length_chips = static_cast(BEIDOU_B1I_CODE_LENGTH_CHIPS); + //d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + } + else if (signal_type == "B3") + { + // GEO Satellites use different secondary code + d_signal_carrier_freq = BEIDOU_B3I_FREQ_HZ; + d_code_period = BEIDOU_B3I_CODE_PERIOD; + d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ; + d_code_length_chips = static_cast(BEIDOU_B3I_CODE_LENGTH_CHIPS); + //d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + } + 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; + 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; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0U; + d_code_samples_per_chip = 0U; + 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_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); + d_carrier_loop_ckf.set_params(1.0, 1.0, static_cast(d_code_period)); + d_carrier_evolution_model.set_code_period(static_cast(d_code_period)); + d_correllator_output_model.set_signal_power(1.0); + d_carrier_loop_ckf.set_model(&d_carrier_evolution_model, &d_correllator_output_model); + + // Initialization of local code replica + // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip + d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + // correlator outputs (scalar) + 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())); + + // 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]; + } + + multicorrelator_cpu.init(2 * trk_parameters.vector_length, d_n_correlator_taps); + + 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 + correlator_data_cpu.init(2 * trk_parameters.vector_length, 1); + correlator_data_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn); + d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + } + else + { + d_data_code = nullptr; + } + + // --- Initializations --- + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn); + // 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 = 0ULL; + d_acq_sample_stamp = 0ULL; + + d_current_prn_length_samples = static_cast(trk_parameters.vector_length); + d_current_correlation_time_s = 0.0; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = std::vector(trk_parameters.cn0_samples); + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_carrier_lock_threshold = trk_parameters.carrier_lock_th; + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_cn0_smoother = Exponential_Smoother(); + if (d_code_period > 0.0) + { + d_cn0_smoother.set_samples_for_initialization(200 / static_cast(d_code_period * 1000.0)); + } + + d_carrier_lock_test_smoother = Exponential_Smoother(); + d_carrier_lock_test_smoother.set_alpha(0.002); + d_carrier_lock_test_smoother.set_min_value(-1.0); + d_carrier_lock_test_smoother.set_offset(0.0); + d_carrier_lock_test_smoother.set_samples_for_initialization(25); + + d_acquisition_gnss_synchro = nullptr; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + 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_code_phase_rate_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_carrier_phase_rate_step_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_state = 0; // initial state: standby + clear_tracking_vars(); + if (trk_parameters.smoother_length > 0) + { + d_carr_ph_history.set_capacity(trk_parameters.smoother_length * 2); + d_code_ph_history.set_capacity(trk_parameters.smoother_length * 2); + } + else + { + d_carr_ph_history.set_capacity(1); + d_code_ph_history.set_capacity(1); + } + + d_dump = trk_parameters.dump; + d_dump_mat = trk_parameters.dump_mat and d_dump; + if (d_dump) + { + d_dump_filename = trk_parameters.dump_filename; + std::string dump_path; + // Get path + if (d_dump_filename.find_last_of('/') != std::string::npos) + { + std::string dump_filename_ = d_dump_filename.substr(d_dump_filename.find_last_of('/') + 1); + dump_path = d_dump_filename.substr(0, d_dump_filename.find_last_of('/')); + d_dump_filename = dump_filename_; + } + else + { + dump_path = std::string("."); + } + if (d_dump_filename.empty()) + { + d_dump_filename = "trk_channel_"; + } + // remove extension if any + if (d_dump_filename.substr(1).find_last_of('.') != std::string::npos) + { + d_dump_filename = d_dump_filename.substr(0, d_dump_filename.find_last_of('.')); + } + + d_dump_filename = dump_path + fs::path::preferred_separator + d_dump_filename; + // create directory + if (!gnss_sdr_create_directory(dump_path)) + { + std::cerr << "GNSS-SDR cannot create dump files for the tracking block. Wrong permissions?" << std::endl; + d_dump = false; + } + } + d_corrected_doppler = false; +} + + +void mixed_veml_tracking::forecast(int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(trk_parameters.vector_length) * 2; + } +} + + +void mixed_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type() == typeid(int)) + { + int tlm_event; + tlm_event = boost::any_cast(pmt::any_ref(msg)); + + switch (tlm_event) + { + case 1: // tlm fault in current channel + { + DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; + gr::thread::scoped_lock lock(d_setlock); + d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition + break; + } + default: + { + break; + } + } + } + } + catch (boost::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any cast!"; + } +} + + +void mixed_veml_tracking::start_tracking() +{ + gr::thread::scoped_lock l(d_setlock); + // correct the code phase according to the delay between acq and trk + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; + d_carrier_phase_rate_step_rad = 0.0; + d_carr_ph_history.clear(); + d_code_ph_history.clear(); + std::array Signal_; + std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); + + if (systemName == "GPS" and signal_type == "1C") + { + gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + } + else if (systemName == "GPS" and signal_type == "2S") + { + gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + } + else if (systemName == "GPS" and signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + } + } + else if (systemName == "Galileo" and signal_type == "1B") + { + if (trk_parameters.track_pilot) + { + std::array pilot_signal = {{'1', 'C', '\0'}}; + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + } + } + else if (systemName == "Galileo" and signal_type == "5X") + { + auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); + std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); + if (trk_parameters.track_pilot) + { + d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + for (uint32_t i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].imag(); + d_data_code[i] = aux_code[i].real(); // the same because it is generated the full signal (E5aI + E5aQ) + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + for (uint32_t i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].real(); + } + } + volk_gnsssdr_free(aux_code); + } + else if (systemName == "Beidou" and signal_type == "B1") + { + beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + // GEO Satellites use different secondary code + if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + { + //d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + //d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + } + + else if (systemName == "Beidou" and signal_type == "B3") + { + beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + // Update secondary code settings for geo satellites + if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + { + //d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + //d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization + d_symbols_per_bit = 1; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + } + + multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code, d_local_code_shift_chips); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + 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_current_correlation_time_s = d_code_period; + + // Initialize tracking ========================================== + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_hz); + d_code_loop_filter.set_update_interval(d_code_period); + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + // Carrier Gaussian filter initialization + d_carrier_loop_ckf.set_params(1.0, 1.0, static_cast(d_code_period)); + d_carrier_loop_ckf.set_state(0.0, static_cast(d_acq_carrier_doppler_hz), 0.0); + d_carrier_loop_ckf.set_state_cov(1.1, 22500.0, 15.0); + + // 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; + DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking pull-in + d_state = 1; + d_cloop = true; + d_pull_in_transitory = true; + d_Prompt_circular_buffer.clear(); + d_corrected_doppler = false; +} + + +mixed_veml_tracking::~mixed_veml_tracking() +{ + if (signal_type == "1C") + { + volk_gnsssdr_free(d_preambles_symbols); + } + + 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_mat) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } + try + { + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_tracking_code); + volk_gnsssdr_free(d_Prompt_Data); + if (trk_parameters.track_pilot) + { + volk_gnsssdr_free(d_data_code); + correlator_data_cpu.free(); + } + multicorrelator_cpu.free(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + +bool mixed_veml_tracking::acquire_secondary() +{ + // ******* preamble correlation ******** + int32_t corr_value = 0; + for (uint32_t i = 0; i < d_secondary_code_length; i++) + { + if (d_Prompt_circular_buffer[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) == static_cast(d_secondary_code_length)) + { + return true; + } + + return false; +} + + +bool mixed_veml_tracking::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; + } + + d_Prompt_buffer[d_cn0_estimation_counter % trk_parameters.cn0_samples] = d_P_accu; + d_cn0_estimation_counter++; + // Code lock indicator + float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); + // Carrier lock indicator + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + // Loss of lock detection + if (!d_pull_in_transitory) + { + if (d_carrier_lock_test < d_carrier_lock_threshold) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) + { + d_carrier_lock_fail_counter--; + } + } + + if (d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + { + d_code_lock_fail_counter++; + } + else + { + if (d_code_lock_fail_counter > 0) + { + d_code_lock_fail_counter--; + } + } + } + if (d_carrier_lock_fail_counter > trk_parameters.max_carrier_lock_fail or d_code_lock_fail_counter > trk_parameters.max_code_lock_fail) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel + << " (carrier_lock_fail_counter:" << d_carrier_lock_fail_counter + << " code_lock_fail_counter : " << d_code_lock_fail_counter << ")"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + return false; + } + return true; +} + + +// correlation requires: +// - updated remnant carrier phase in radians (rem_carr_phase_rad) +// - updated remnant code phase in samples (d_rem_code_phase_samples) +// - d_code_freq_chips +// - d_carrier_doppler_hz +void mixed_veml_tracking::do_correlation_step(const gr_complex *input_samples) +{ + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), + trk_parameters.vector_length); + + // DATA CORRELATOR (if tracking tracks the pilot signal) + if (trk_parameters.track_pilot) + { + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); + correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), + trk_parameters.vector_length); + } +} + + +void mixed_veml_tracking::run_dll_pll() +{ + // ################## PLL ########################################################## + // PLL discriminator + if (d_cloop) + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_phase_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_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; +# if 1 + } + + if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) + { + // FLL discriminator + // d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + + d_P_accu_old = d_P_accu; + // std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; + // Carrier discriminator filter + if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) + { + // pure FLL, disable PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); + } + else + { + // FLL-aided PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); + } + } + else + { + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(0, d_carr_phase_error_hz, d_current_correlation_time_s); +#endif + + // Carrier correlator output filter + std::pair d_carrier_nco = d_carrier_loop_ckf.get_carrier_nco(*d_Prompt_Data); + + // New carrier Doppler frequency estimation + // d_carr_error_filt_hz = d_carrier_nco.first; + d_carrier_doppler_hz = d_carrier_nco.second; + + } + + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_carr_error_filt_hz; + + // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; + // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; + + // ################## 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.apply(d_code_error_chips); // [chips/second] + // New code Doppler frequency estimation + d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; + + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler + if (trk_parameters.enable_doppler_correction == true) + { + if (d_pull_in_transitory == false and d_corrected_doppler == false) + { + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + + if (d_dll_filt_history.full()) + { + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); + LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); + d_carrier_loop_ckf.set_state(0.0, static_cast(d_carrier_doppler_hz - carrier_doppler_error_hz), 0.0); + d_carrier_loop_ckf.set_state_cov(1.1, 22500.0, 15.0); + d_corrected_doppler = true; + } + d_dll_filt_history.clear(); + } + } + } +} + + +void mixed_veml_tracking::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[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + } + d_P_accu_old = gr_complex(0.0, 0.0); + d_carr_phase_error_hz = 0.0; + d_carr_freq_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_current_data_symbol = 0; + d_Prompt_circular_buffer.clear(); + d_carrier_phase_rate_step_rad = 0.0; + d_code_phase_rate_step_chips = 0.0; + d_carr_ph_history.clear(); + d_code_ph_history.clear(); +} + + +void mixed_veml_tracking::update_tracking_vars() +{ + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + + // ################## 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; + d_current_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples + + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; + // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] + if (trk_parameters.high_dyn) + { + d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_prn_length_samples))); + if (d_carr_ph_history.full()) + { + double tmp_cp1 = 0.0; + double tmp_cp2 = 0.0; + double tmp_samples = 0.0; + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) + { + tmp_cp1 += d_carr_ph_history[k].first; + tmp_cp2 += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; + } + tmp_cp1 /= static_cast(trk_parameters.smoother_length); + tmp_cp2 /= static_cast(trk_parameters.smoother_length); + d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; + } + } + // std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); + + // carrier phase accumulator + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; + d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * 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; + if (trk_parameters.high_dyn) + { + d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_prn_length_samples))); + if (d_code_ph_history.full()) + { + double tmp_cp1 = 0.0; + double tmp_cp2 = 0.0; + double tmp_samples = 0.0; + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) + { + tmp_cp1 += d_code_ph_history[k].first; + tmp_cp2 += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; + } + tmp_cp1 /= static_cast(trk_parameters.smoother_length); + tmp_cp2 /= static_cast(trk_parameters.smoother_length); + d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples; + } + } + // 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 mixed_veml_tracking::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; + } + + // data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (trk_parameters.track_pilot) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + + d_current_data_symbol++; + // data secondary code roll-up + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu += *d_Prompt; + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + } + d_current_data_symbol++; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu = *d_Prompt_Data; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + + if (trk_parameters.track_pilot) + { + // If tracking pilot, disable Costas loop + d_cloop = false; + } + else + { + d_cloop = true; + } +} + + +void mixed_veml_tracking::log_data() +{ + if (d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + uint64_t tmp_long_int; + if (trk_parameters.track_pilot) + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } + 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); + + 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 + tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); + // 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)); + // carrier phase rate [Hz/s] + tmp_float = d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2; + 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)); + // code phase rate [chips/s^2] + tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = d_carr_phase_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 + uint32_t prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } +} + + +int32_t mixed_veml_tracking::save_matfile() +{ + // READ DUMP FILE + std::ifstream::pos_type size; + int32_t number_of_double_vars = 1; + int32_t number_of_float_vars = 19; + int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(float) * number_of_float_vars + sizeof(uint32_t); + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + // add channel number to the filename + dump_filename_.append(std::to_string(d_channel)); + // add extension + dump_filename_.append(".dat"); + std::cout << "Generating .mat file for " << dump_filename_ << std::endl; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(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 + int64_t 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; + } + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); + try + { + if (dump_file.is_open()) + { + for (int64_t 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(uint64_t)); + 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(&carrier_doppler_rate_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_rate_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(uint32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 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.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 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.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + return 0; +} + + +void mixed_veml_tracking::set_channel(uint32_t channel) +{ + gr::thread::scoped_lock l(d_setlock); + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump) + { + std::string dump_filename_ = d_dump_filename; + // add channel number to the filename + dump_filename_.append(std::to_string(d_channel)); + // add extension + dump_filename_.append(".dat"); + + 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(dump_filename_.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void mixed_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) +{ + gr::thread::scoped_lock l(d_setlock); + d_acquisition_gnss_synchro = p_gnss_synchro; +} + + +void mixed_veml_tracking::stop_tracking() +{ + gr::thread::scoped_lock l(d_setlock); + d_state = 0; +} + + +int mixed_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + gr::thread::scoped_lock l(d_setlock); + const auto *in = reinterpret_cast(input_items[0]); + auto **out = reinterpret_cast(&output_items[0]); + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; + + if (d_pull_in_transitory == true) + { + if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + } + } + switch (d_state) + { + case 0: // Standby - Consume samples at full throttle, do nothing + { + d_sample_counter += static_cast(ninput_items[0]); + consume_each(ninput_items[0]); + return 0; + break; + } + case 1: // Pull-in + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + double delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; + + d_code_freq_chips = d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + d_code_phase_rate_step_chips = 0.0; + 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_acq_code_phase_samples = T_prn_mod_samples - std::fmod(delta_trk_to_acq_prn_start_samples, T_prn_mod_samples); + d_current_prn_length_samples = round(T_prn_mod_samples); + + int32_t samples_offset = round(d_acq_code_phase_samples); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; + d_sample_counter += samples_offset; // count for the processed samples + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); + + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + consume_each(samples_offset); // shift input to perform alignment with local replica + return 0; + } + case 2: // Wide tracking and symbol synchronization + { + do_correlation_step(in); + // 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; + + //fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period + if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + // Check lock status + 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(); + + if (!d_pull_in_transitory) + { + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else + { + next_state = true; + } + } + else + { + next_state = false; //keep in state 2 during pull-in transitory + } + 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_P_data_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_Prompt_circular_buffer.clear(); + d_current_symbol = 0; + d_current_data_symbol = 0; + + if (d_enable_extended_integration) + { + // UPDATE INTEGRATION TIME + d_extend_correlation_symbols_count = 0; + d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + 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_update_interval(d_current_correlation_time_s); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); + d_carrier_loop_ckf.set_params(1.0, 1.0, static_cast(d_code_period)); + 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: // coherent integration (correlation time extension) + { + // perform a correlation step + do_correlation_step(in); + save_correlation_results(); + update_tracking_vars(); + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.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.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + 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; + } + break; + } + case 4: // narrow tracking + { + // perform a correlation step + do_correlation_step(in); + 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(); + if (d_current_data_symbol == 0) + { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.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.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + // 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 + } + } + } + } + consume_each(d_current_prn_length_samples); + d_sample_counter += static_cast(d_current_prn_length_samples); + 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; + *out[0] = current_synchro_data; + return 1; + } + return 0; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h new file mode 100644 index 000000000..1478a4ad4 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h @@ -0,0 +1,266 @@ +/*! + * \file mixed_veml_tracking.h + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_MIXED_VEML_TRACKING_H +#define GNSS_SDR_MIXED_VEML_TRACKING_H + +//#include "tracking_models.h" +#include "tracking_Gaussian_filter.h" + +#include "cpu_multicorrelator_real_codes.h" +#include "dll_pll_conf.h" +#include "exponential_smoother.h" +#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter +#include "tracking_loop_filter.h" // for DLL filter +#include +#include // for boost::shared_ptr +#include // for block +#include // for gr_complex +#include // for gr_vector_int, gr_vector... +#include // for pmt_t +#include // for int32_t +#include // for string, ofstream +#include // for pair +#include + +class CarrierTransitionModel : public ModelFunction +{ +public: + // explicit CarrierTransitionModel(const float carrier_pdi) { pdi = carrier_pdi; }; + arma::vec operator()(const arma::vec& input) override { + arma::vec output = arma::zeros(3,1); + output(0, 0) = input(0) + PI_2*pdi*input(1) + 0.5*PI_2*std::pow(pdi, 2)*input(2); + output(0, 0) = input(1) + pdi*input(2); + output(0, 0) = input(2); + + return output; + }; + void set_code_period(const float carrier_pdi) { pdi = carrier_pdi; }; + +private: + arma::mat t_mat; + float pdi; +}; + +class CarrierMeasurementModel : public ModelFunction +{ +public: + // explicit CarrierMeasurementModel(const float carrier_pow) { alpha = carrier_pow; }; + arma::vec operator()(const arma::vec& input) override { + using namespace std::complex_literals; + std::complex output_iq = static_cast(alpha) * std::exp( 1i * static_cast(input(0)) ); + + arma::vec output = arma::zeros(2,1); + output(0, 0) = static_cast( std::real( output_iq ) ); + output(1, 0) = static_cast( std::imag( output_iq ) ); + + return output; + }; + void set_signal_power(const float carrier_pow) { alpha = carrier_pow; }; + +private: + float alpha; +}; + + +class Gnss_Synchro; +class mixed_veml_tracking; + +using mixed_veml_tracking_sptr = boost::shared_ptr; + +mixed_veml_tracking_sptr mixed_veml_make_tracking(const Dll_Pll_Conf &conf_); + +/*! + * \brief This class implements a code DLL + carrier PLL tracking block. + */ +class mixed_veml_tracking : public gr::block +{ +public: + ~mixed_veml_tracking(); + + void set_channel(uint32_t channel); + void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); + void start_tracking(); + void stop_tracking(); + + int general_work(int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast(int noutput_items, gr_vector_int &ninput_items_required); + + CarrierTransitionModel d_carrier_evolution_model; + CarrierMeasurementModel d_correllator_output_model; + +private: + friend mixed_veml_tracking_sptr mixed_veml_make_tracking(const Dll_Pll_Conf &conf_); + void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); + mixed_veml_tracking(const Dll_Pll_Conf &conf_); + + bool cn0_and_tracking_lock_status(double coh_integration_time_s); + bool acquire_secondary(); + void do_correlation_step(const gr_complex *input_samples); + void run_dll_pll(); + void update_tracking_vars(); + void clear_tracking_vars(); + void save_correlation_results(); + void log_data(); + int32_t save_matfile(); + + // tracking configuration vars + Dll_Pll_Conf trk_parameters; + bool d_veml; + bool d_cloop; + uint32_t d_channel; + Gnss_Synchro *d_acquisition_gnss_synchro; + + // Signal parameters + bool d_secondary; + double d_signal_carrier_freq; + double d_code_period; + double d_code_chip_rate; + uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; + uint32_t d_code_length_chips; + uint32_t 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) + int32_t d_symbols_per_bit; + std::string systemName; + std::string signal_type; + std::string *d_secondary_code_string; + std::string *d_data_secondary_code_string; + std::string signal_pretty_name; + + int32_t *d_preambles_symbols; + int32_t d_preamble_length_symbols; + + // dll filter buffer + boost::circular_buffer d_dll_filt_history; + // tracking state machine + int32_t d_state; + + // Integration period in samples + int32_t d_correlation_length_ms; + int32_t d_n_correlator_taps; + + float *d_tracking_code; + float *d_data_code; + float *d_local_code_shift_chips; + float *d_prompt_data_shift; + Cpu_Multicorrelator_Real_Codes multicorrelator_cpu; + Cpu_Multicorrelator_Real_Codes correlator_data_cpu; //for data channel + + /* TODO: currently the multicorrelator does not support adding extra correlator + with different local code, thus we need extra multicorrelator instance. + Implement this functionality inside multicorrelator class + as an enhancement to increase the performance + */ + 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; + int32_t d_extend_correlation_symbols_count; + int32_t d_current_symbol; + int32_t d_current_data_symbol; + + gr_complex d_VE_accu; + gr_complex d_E_accu; + gr_complex d_P_accu; + gr_complex d_P_accu_old; + gr_complex d_L_accu; + gr_complex d_VL_accu; + + gr_complex d_P_data_accu; + gr_complex *d_Prompt_Data; + + double d_code_phase_step_chips; + double d_code_phase_rate_step_chips; + boost::circular_buffer> d_code_ph_history; + double d_carrier_phase_step_rad; + double d_carrier_phase_rate_step_rad; + boost::circular_buffer> d_carr_ph_history; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + float d_rem_carr_phase_rad; + + TrackingNonlinearFilter d_carrier_loop_ckf; + Tracking_loop_filter d_code_loop_filter; + Tracking_FLL_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + + // tracking vars + bool d_pull_in_transitory; + bool d_corrected_doppler; + double d_current_correlation_time_s; + double d_carr_phase_error_hz; + double d_carr_freq_error_hz; + double d_carr_error_filt_hz; + double d_code_error_chips; + double d_code_error_filt_chips; + double d_code_freq_chips; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_rem_code_phase_chips; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; + // PRN period in samples + int32_t d_current_prn_length_samples; + // processing samples counters + uint64_t d_sample_counter; + uint64_t d_acq_sample_stamp; + + // CN0 estimation and lock detector + int32_t d_cn0_estimation_counter; + int32_t d_carrier_lock_fail_counter; + int32_t d_code_lock_fail_counter; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + boost::circular_buffer d_Prompt_circular_buffer; + std::vector d_Prompt_buffer; + Exponential_Smoother d_cn0_smoother; + Exponential_Smoother d_carrier_lock_test_smoother; + // file dump + std::ofstream d_dump_file; + std::string d_dump_filename; + bool d_dump; + bool d_dump_mat; +}; + +#endif // GNSS_SDR_MIXED_VEML_TRACKING_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 77178d036..fe6f9a65a 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -29,6 +29,7 @@ set(TRACKING_LIB_SOURCES tracking_discriminators.cc tracking_FLL_PLL_filter.cc tracking_loop_filter.cc + tracking_Gaussian_filter.cc dll_pll_conf.cc bayesian_estimation.cc exponential_smoother.cc @@ -46,9 +47,11 @@ set(TRACKING_LIB_HEADERS tracking_discriminators.h tracking_FLL_PLL_filter.h tracking_loop_filter.h + tracking_Gaussian_filter.h dll_pll_conf.h bayesian_estimation.h exponential_smoother.h + tracking_models.h ) @@ -67,7 +70,7 @@ if(ENABLE_CUDA) endif() endif() -if(ARMADILLO_VERSION_STRING VERSION_GREATER 7.400) +if(ARMADILLO_VERSION_STRING VERSION_GREATER_EQUAL 7.400) # sqrtmat_sympd() requires 7.400 set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} nonlinear_tracking.cc) set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} nonlinear_tracking.h) diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.cc b/src/algorithms/tracking/libs/nonlinear_tracking.cc index 2377bf860..f93fd1627 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.cc +++ b/src/algorithms/tracking/libs/nonlinear_tracking.cc @@ -132,7 +132,6 @@ void CubatureFilter::predict_sequential(const arma::vec& x_post, const arma::mat // Propagate and evaluate cubature points arma::vec Xi_post; arma::vec Xi_pred; - for (uint8_t i = 0; i < np; i++) { Xi_post = Sm_post * (std::sqrt(static_cast(np) / 2.0) * gen_one.col(i)) + x_post; diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.h b/src/algorithms/tracking/libs/nonlinear_tracking.h index 26f321463..e2e7f3778 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.h +++ b/src/algorithms/tracking/libs/nonlinear_tracking.h @@ -45,6 +45,7 @@ #include #include +//#include "tracking_models.h" // Abstract model function class ModelFunction @@ -55,6 +56,7 @@ public: virtual ~ModelFunction() = default; }; + class GaussianFilter { public: diff --git a/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc b/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc new file mode 100644 index 000000000..72c5b2ba4 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc @@ -0,0 +1,77 @@ +/*! + * \file tracking_Gaussian_filter.cc + * \brief Implementation of a nonlinear Gaussian filter for tracking carrier loop. + * + * Class that implements a nonlinear Gaussian for tracking carrier loop using + * CKF or UKF implementations. + * + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. + * + * \authors
    + *
  • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
  • Jordi Vila-Valls 2019. jvila(at)cttc.es + *
+ * ------------------------------------------------------------------------- + * + * 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 "tracking_Gaussian_filter.h" + +void TrackingGaussianFilter::set_ncov_measurement(float ev_1, float ev_2) +{ + ncov_measurement = arma::zeros(2, 2); + ncov_measurement(0, 0) = ev_1; + ncov_measurement(1, 1) = ev_2; +} + +void TrackingGaussianFilter::set_ncov_process(float ev_1, float ev_2, float ev_3) +{ + ncov_process = arma::zeros(3, 3); + ncov_process(0, 0) = ev_1; + ncov_process(1, 1) = ev_2; + ncov_process(2, 2) = ev_3; +} + +void TrackingGaussianFilter::set_params(float ev_1, float ev_2, float pdi_carr) +{ + set_ncov_measurement(ev_1, ev_2); + set_ncov_process(std::pow(pdi_carr, 6), std::pow(pdi_carr, 4), std::pow(pdi_carr, 2)); +} + +void TrackingGaussianFilter::set_state(float x_1, float x_2, float x_3) +{ + state = arma::zeros(3,1); + state(0) = x_1; + state(1) = x_2; + state(2) = x_3; +} + +void TrackingGaussianFilter::set_state_cov(float P_x_1, float P_x_2, float P_x_3) +{ + state_cov = arma::zeros(3,3); + state_cov(0, 0) = P_x_1; + state_cov(1, 1) = P_x_2; + state_cov(2, 2) = P_x_3; +} diff --git a/src/algorithms/tracking/libs/tracking_Gaussian_filter.h b/src/algorithms/tracking/libs/tracking_Gaussian_filter.h new file mode 100644 index 000000000..7888fa111 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_Gaussian_filter.h @@ -0,0 +1,136 @@ +/*! + * \file tracking_Gaussian_filter.h + * \brief Implementation of a Gaussian filter for tracking carrier loop. + * + * Class that implements a Gaussian for tracking carrier loop using + * CKF or UKF implementations. + * + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. + * + * \authors
    + *
  • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
  • Jordi Vila-Valls 2019. jvila(at)cttc.es + *
+ * ------------------------------------------------------------------------- + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_TRACKING_GAUSSIAN_FILTER_H_ +#define GNSS_SDR_TRACKING_GAUSSIAN_FILTER_H_ + +#include +#include +// #include "tracking_models.h" +#include "nonlinear_tracking.h" +#include "MATH_CONSTANTS.h" + +/*! + * \brief This class implements a standard Gaussian filter for carrier tracking. + * + * The algorithm is described in: + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. + */ +class TrackingGaussianFilter +{ +public: + // TrackingGaussianFilter(); + // TrackingGaussianFilter(float pdi_carr, uint32_t kf_order, bool kf_extended); + // ~TrackingGaussianFilter(); + + // void initialize(); + //void initialize(float pdi_carar, arma::colvec x_ini, arma::mat P_x_ini, arma::mat Q_ini, arma::mat R_ini, ModelFunction* f_t, ModelFunction* f_m); + + //void set_ncov_process(float err_variance); + void set_ncov_process(float ev_1, float ev_2, float ev_3); + + // void set_ncov_measurement(float err_variance); + void set_ncov_measurement(float ev_1, float ev_2); + + void set_params(float ev_1, float ev_2, float pdi_carr); + + // void set_KF_x(float x_1, float x_2); + void set_state(float x_1, float x_2, float x_3); + + //void set_KF_P_x(float P_x_1, float P_x_2); + void set_state_cov(float P_x_1, float P_x_2, float P_x_3); + + // void setup_filter(float pdi_carr, uint32_t kf_order, bool kf_extended); + +protected: + + //float d_pdi_carr = 0.0; + // uint32_t d_kf_order = 2U; + // bool d_kf_extended = false; + + // Kalman filter parameters + // arma::colvec kf_x_ini; /* initial state vector */ + // arma::mat kf_P_x_ini; /* initial state error covariance matrix */ + + arma::colvec state; /* state vector */ + arma::mat state_cov; /* state error covariance matrix */ + + // arma::colvec kf_x_pre; /* predicted state vector */ + // arma::mat kf_P_x_pre; /* Predicted state error covariance matrix */ + + // arma::mat kf_P_y; /* innovation covariance matrix */ + + // arma::mat kf_F; /* state transition matrix */ + // arma::mat kf_H; /* system matrix */ + + arma::mat ncov_process; /* model error covariance matrix */ + arma::mat ncov_measurement; /* measurement error covariance matrix */ + + // arma::colvec kf_z; /* measurement vector */ + // arma::colvec kf_y; /* innovation vector */ + // arma::mat kf_K; /* Kalman gain matrix */ + +}; + +template +class TrackingNonlinearFilter : public TrackingGaussianFilter +{ +public: + //carr_nco get_carrier_nco(float PLL_discriminator, float pll_bw_hz); + std::pair get_carrier_nco(const gr_complex Prompt); + + void set_transition_model(ModelFunction* ft) { func_transition = ft; }; + void set_measurement_model(ModelFunction* fm) { func_measurement = fm; }; + void set_model(ModelFunction* ft, ModelFunction* fm) { + set_transition_model(ft); + set_measurement_model(fm); + }; + +private: + ModelFunction* func_transition; + ModelFunction* func_measurement; + + NonlinearFilter GaussFilt; + // CubatureFilter GaussFilt; + +}; +#include "tracking_Gaussian_filter.tcc" + +#endif diff --git a/src/algorithms/tracking/libs/tracking_Gaussian_filter.tcc b/src/algorithms/tracking/libs/tracking_Gaussian_filter.tcc new file mode 100644 index 000000000..012277d09 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_Gaussian_filter.tcc @@ -0,0 +1,58 @@ + +#ifndef GNSS_SDR_TRACKING_GAUSSIAN_FILTER_TCC_ +#define GNSS_SDR_TRACKING_GAUSSIAN_FILTER_TCC_ + +#include +#include +#include "MATH_CONSTANTS.h" + +/* + * Gaussian filter operating on correlator outputs + * Req Input in [Hz/Ti] + * The output is in [Hz/s]. + */ +template +// carr_nco TrackingNonlinearFilter::get_carrier_nco(gr_complex Prompt) +std::pair TrackingNonlinearFilter::get_carrier_nco(const gr_complex Prompt) +{ + arma::colvec state_pred; + arma::mat state_cov_pred; + + // State Update +# if 0 + std::cout << "state:"; + state.print(); + std::cout << std::endl; + std::cout << "state_cov:"; + state_cov.print(); + std::cout << std::endl; + std::cout << "ncov_process:"; + ncov_process.print(); + std::cout << std::endl; + + std::cout << "BEGIN PREDICT" << std::endl; +#endif + GaussFilt.predict_sequential(state, state_cov, func_transition, ncov_process); + state_pred = GaussFilt.get_x_pred(); + state_cov_pred = GaussFilt.get_P_x_pred(); + + // Measurement Update + arma::colvec measurement = arma::zeros(2, 1); + measurement(0) = Prompt.real(); + measurement(1) = Prompt.imag(); + + /* + arma::colvec innovation = arma::zeros(2, 1); + innovation(0) = measurement(0) - std::sqrt(d_carrier_power_snv) * cos(state_pred(0)); + innovation(1) = measurement(1) - std::sqrt(d_carrier_power_snv) * sin(state_pred(0)); + */ + + GaussFilt.update_sequential(measurement, state_pred, state_cov_pred, func_measurement, ncov_measurement); + state = GaussFilt.get_x_pred(); + state_cov = GaussFilt.get_P_x_pred(); + + // Set a new carrier estimation to the NCO + return std::make_pair(static_cast(state(0)), static_cast(state(1))); +} + +#endif diff --git a/src/algorithms/tracking/libs/tracking_models.h b/src/algorithms/tracking/libs/tracking_models.h new file mode 100644 index 000000000..a11d27a93 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_models.h @@ -0,0 +1,97 @@ +/*! + * \file tracking_models.h + * \brief Implementation of linear and nonlinear tracking models. + * \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu + * + * ------------------------------------------------------------------------- + * + * 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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_TRACKING_MODELS_H_ +#define GNSS_SDR_TRACKING_MODELS_H_ + +#include "cpu_multicorrelator_real_codes.h" +#include "dll_pll_conf.h" +#include "exponential_smoother.h" +#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter +#include "tracking_loop_filter.h" // for DLL filter +#include +#include // for boost::shared_ptr +#include // for block +#include // for gr_complex +#include // for gr_vector_int, gr_vector... +#include +#include // for int32_t +#include // for string, ofstream +#include // for pair +#include + +// Abstract model function +class ModelFunction +{ +public: + ModelFunction(){}; + virtual arma::vec operator()(const arma::vec& input) = 0; + virtual ~ModelFunction() = default; +}; + +class CarrierTransitionModel : public ModelFunction +{ +public: + explicit CarrierTransitionModel(const float carrier_pdi) { pdi = carrier_pdi; }; + arma::vec operator()(const arma::vec& input) override { + arma::vec output = arma::zeros(3,1); + output(0, 0) = input(0) + PI_2*pdi*input(1) + 0.5*PI_2*std::pow(pdi, 2)*input(2); + output(0, 0) = input(1) + pdi*input(2); + output(0, 0) = input(2); + + return output; + }; + +private: + arma::mat t_mat; + float pdi; +}; + +class CarrierMeasurementModel : public ModelFunction +{ +public: + explicit CarrierMeasurementModel(const float carrier_pow) { alpha = carrier_pow; }; + arma::vec operator()(const arma::vec& input) override { + using namespace std::complex_literals; + std::complex output_iq = static_cast(alpha) * std::exp( 1i * static_cast(input(0)) ); + + arma::vec output = arma::zeros(2,1); + output(0, 0) = static_cast( std::real( output_iq ) ); + output(1, 0) = static_cast( std::imag( output_iq ) ); + + return output; + }; + +private: + float alpha; +}; + +#endif diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 7976d99da..f7c1a32fe 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -53,6 +53,7 @@ #include "fir_filter.h" #include "freq_xlating_fir_filter.h" #include "galileo_e1_dll_pll_veml_tracking.h" +#include "galileo_e1_mixed_veml_tracking.h" #include "galileo_e1_pcps_8ms_ambiguous_acquisition.h" #include "galileo_e1_pcps_ambiguous_acquisition.h" #include "galileo_e1_pcps_cccwsr_ambiguous_acquisition.h" @@ -1791,6 +1792,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation == "Galileo_E1_Mixed_VEML_Tracking") + { + std::unique_ptr block_(new GalileoE1MixedVemlTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } #if ENABLE_FPGA else if (implementation == "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga") { @@ -2167,6 +2174,12 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams)); block = std::move(block_); } + else if (implementation == "Galileo_E1_Mixed_VEML_Tracking") + { + std::unique_ptr block_(new GalileoE1MixedVemlTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } #if ENABLE_FPGA else if (implementation == "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga") { diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 385e7f2ee..d201a53cf 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -812,6 +812,7 @@ if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) endif() add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_mixed_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index e20338cfa..61b789ab9 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -103,6 +103,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc" #include "unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc" #endif +#include "unit-tests/signal-processing-blocks/tracking/galileo_e1_mixed_veml_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc" #include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc" @@ -138,7 +139,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" -#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_mixed_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc" #if FPGA_BLOCKS_TEST diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc index a0d42aeb2..46d9ea09b 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -28,6 +28,7 @@ * ------------------------------------------------------------------------- */ +// #include "tracking_models.h" #include "nonlinear_tracking.h" #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_mixed_veml_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_mixed_veml_tracking_test.cc new file mode 100644 index 000000000..ca6c2f5d2 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_mixed_veml_tracking_test.cc @@ -0,0 +1,214 @@ +/*! + * \file galileo_e1_mixed_veml_tracking_test.cc + * \brief This class implements a tracking test for GalileoE1MixedVemlTracking + * class based on some input parameters. + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-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 "galileo_e1_mixed_veml_tracking.h" +#include "gnss_block_factory.h" +#include "gnss_block_interface.h" +#include "gnss_sdr_valve.h" +#include "gnss_synchro.h" +#include "in_memory_configuration.h" +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef GR_GREATER_38 +#include +#else +#include +#endif + +class GalileoE1MixedVemlTrackingInternalTest : public ::testing::Test +{ +protected: + GalileoE1MixedVemlTrackingInternalTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + stop = false; + message = 0; + gnss_synchro = Gnss_Synchro(); + } + + ~GalileoE1MixedVemlTrackingInternalTest() override = default; + + void init(); + + gr::msg_queue::sptr queue; + gr::top_block_sptr top_block; + std::shared_ptr factory; + std::shared_ptr config; + Gnss_Synchro gnss_synchro{}; + size_t item_size; + bool stop; + int message; +}; + + +void GalileoE1MixedVemlTrackingInternalTest::init() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'E'; + std::string signal = "1B"; + signal.copy(gnss_synchro.Signal, 2, 0); + gnss_synchro.PRN = 11; + + config->set_property("GNSS-SDR.internal_fs_sps", "8000000"); + config->set_property("Tracking_1B.implementation", "Galileo_E1_Mixed_VEML_Tracking"); + config->set_property("Tracking_1B.item_type", "gr_complex"); + config->set_property("Tracking_1B.dump", "false"); + config->set_property("Tracking_1B.dump_filename", "../data/veml_tracking_ch_"); + config->set_property("Tracking_1B.early_late_space_chips", "0.15"); + config->set_property("Tracking_1B.very_early_late_space_chips", "0.6"); + config->set_property("Tracking_1B.pll_bw_hz", "30.0"); + config->set_property("Tracking_1B.dll_bw_hz", "2.0"); +} + + +TEST_F(GalileoE1MixedVemlTrackingInternalTest, Instantiate) +{ + init(); + auto tracking = factory->GetBlock(config, "Tracking_1B", "Galileo_E1_Mixed_VEML_Tracking", 1, 1); + EXPECT_STREQ("Galileo_E1_Mixed_VEML_Tracking", tracking->implementation().c_str()); +} + + +TEST_F(GalileoE1MixedVemlTrackingInternalTest, ConnectAndRun) +{ + int fs_in = 8000000; + int nsamples = 40000000; + std::chrono::time_point start, end; + std::chrono::duration elapsed_seconds(0); + init(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Tracking test"); + + // Example using smart pointers and the block factory + std::shared_ptr trk_ = factory->GetBlock(config, "Tracking_1B", "Galileo_E1_Mixed_VEML_Tracking", 1, 1); + std::shared_ptr tracking = std::dynamic_pointer_cast(trk_); + + ASSERT_NO_THROW({ + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel."; + + ASSERT_NO_THROW({ + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."; + + ASSERT_NO_THROW({ + tracking->connect(top_block); + gr::analog::sig_source_c::sptr source = gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0)); + boost::shared_ptr valve = gnss_sdr_make_valve(sizeof(gr_complex), nsamples, queue); + gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + top_block->connect(source, 0, valve, 0); + top_block->connect(valve, 0, tracking->get_left_block(), 0); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + }) << "Failure connecting the blocks of tracking test."; + + tracking->start_tracking(); + + EXPECT_NO_THROW({ + start = std::chrono::system_clock::now(); + top_block->run(); //Start threads and wait + end = std::chrono::system_clock::now(); + elapsed_seconds = end - start; + }) << "Failure running the top_block."; + + std::cout << "Processed " << nsamples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; +} + + +TEST_F(GalileoE1MixedVemlTrackingInternalTest, ValidationOfResults) +{ + std::chrono::time_point start, end; + std::chrono::duration elapsed_seconds(0); + // int num_samples = 40000000; // 4 Msps + // unsigned int skiphead_sps = 24000000; // 4 Msps + int num_samples = 80000000; // 8 Msps + unsigned int skiphead_sps = 8000000; // 8 Msps + init(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Tracking test"); + + // Example using smart pointers and the block factory + std::shared_ptr trk_ = factory->GetBlock(config, "Tracking_1B", "Galileo_E1_Mixed_VEML_Tracking", 1, 1); + std::shared_ptr tracking = std::dynamic_pointer_cast(trk_); + + // gnss_synchro.Acq_delay_samples = 1753; // 4 Msps + // gnss_synchro.Acq_doppler_hz = -9500; // 4 Msps + gnss_synchro.Acq_delay_samples = 17256; // 8 Msps + gnss_synchro.Acq_doppler_hz = -8750; // 8 Msps + gnss_synchro.Acq_samplestamp_samples = 0; + + ASSERT_NO_THROW({ + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel."; + + ASSERT_NO_THROW({ + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."; + + ASSERT_NO_THROW({ + tracking->connect(top_block); + }) << "Failure connecting tracking to the top_block."; + + ASSERT_NO_THROW({ + std::string path = std::string(TEST_PATH); + std::string file = path + "signal_samples/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat"; + const char* file_name = file.c_str(); + gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(gr_complex), file_name, false); + gr::blocks::skiphead::sptr skip_head = gr::blocks::skiphead::make(sizeof(gr_complex), skiphead_sps); + boost::shared_ptr valve = gnss_sdr_make_valve(sizeof(gr_complex), num_samples, queue); + gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + top_block->connect(file_source, 0, skip_head, 0); + top_block->connect(skip_head, 0, valve, 0); + top_block->connect(valve, 0, tracking->get_left_block(), 0); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + }) << "Failure connecting the blocks of tracking test."; + + tracking->start_tracking(); + + EXPECT_NO_THROW({ + start = std::chrono::system_clock::now(); + top_block->run(); // Start threads and wait + end = std::chrono::system_clock::now(); + elapsed_seconds = end - start; + }) << "Failure running the top_block."; + + std::cout << "Tracked " << num_samples << " samples in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; +} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc index 61ae638d0..82286f3d3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -28,6 +28,7 @@ * ------------------------------------------------------------------------- */ +// #include "tracking_models.h" #include "nonlinear_tracking.h" #include #include