diff --git a/src/algorithms/acquisition/adapters/CMakeLists.txt b/src/algorithms/acquisition/adapters/CMakeLists.txt index c0900d4ff..f8c6b34b1 100644 --- a/src/algorithms/acquisition/adapters/CMakeLists.txt +++ b/src/algorithms/acquisition/adapters/CMakeLists.txt @@ -31,6 +31,7 @@ set(ACQ_ADAPTER_SOURCES galileo_e1_pcps_tong_ambiguous_acquisition.cc galileo_e1_pcps_8ms_ambiguous_acquisition.cc galileo_e5a_noncoherent_iq_acquisition_caf.cc + glonass_l1_ca_pcps_acquisition.cc ) if(ENABLE_FPGA) diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc new file mode 100644 index 000000000..2ef2828f1 --- /dev/null +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -0,0 +1,360 @@ +#include "glonass_l1_ca_pcps_acquisition.h" +#include +#include +#include "glonass_l1_signal_processing.h" +#include "Glonass_L1_CA.h" +#include "configuration_interface.h" + + +using google::LogMessage; +GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + configuration_ = configuration; + std::string default_item_type = "gr_complex"; + std::string default_dump_filename = "./data/acquisition.dat"; + + DLOG(INFO) << "role " << role; + + item_type_ = configuration_->property(role + ".item_type", default_item_type); + + fs_in_ = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); + if_ = configuration_->property(role + ".if", 0); + dump_ = configuration_->property(role + ".dump", false); + doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); + + bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); + use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions + + max_dwells_ = configuration_->property(role + ".max_dwells", 1); + + dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); + + //--- Find number of samples per spreading code ------------------------- + code_length_ = round(fs_in_ / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); + + vector_length_ = code_length_ * sampled_ms_; + + if( bit_transition_flag_ ) + { + vector_length_ *= 2; + } + + code_ = new gr_complex[vector_length_]; + + if (item_type_.compare("cshort") == 0 ) + { + item_size_ = sizeof(lv_16sc_t); + acquisition_sc_ = pcps_make_acquisition_sc(sampled_ms_, max_dwells_, + doppler_max_, if_, fs_in_, code_length_, code_length_, + bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, dump_filename_); + DLOG(INFO) << "acquisition(" << acquisition_sc_->unique_id() << ")"; + + }else{ + item_size_ = sizeof(gr_complex); + acquisition_cc_ = pcps_make_acquisition_cc(sampled_ms_, max_dwells_, + doppler_max_, if_, fs_in_, code_length_, code_length_, + bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, dump_filename_); + DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")"; + } + + stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); + DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + + if (item_type_.compare("cbyte") == 0) + { + cbyte_to_float_x2_ = make_complex_byte_to_float_x2(); + float_to_complex_ = gr::blocks::float_to_complex::make(); + } + + channel_ = 0; + threshold_ = 0.0; + doppler_step_ = 0; + gnss_synchro_ = 0; +} + + +GlonassL1CaPcpsAcquisition::~GlonassL1CaPcpsAcquisition() +{ + delete[] code_; +} + + +void GlonassL1CaPcpsAcquisition::set_channel(unsigned int channel) +{ + channel_ = channel; + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_channel(channel_); + } + else + { + acquisition_cc_->set_channel(channel_); + } +} + + +void GlonassL1CaPcpsAcquisition::set_threshold(float threshold) +{ + float pfa = configuration_->property(role_ + ".pfa", 0.0); + + if(pfa == 0.0) + { + threshold_ = threshold; + } + else + { + threshold_ = calculate_threshold(pfa); + } + + DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_; + + + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_threshold(threshold_); + } + else + { + acquisition_cc_->set_threshold(threshold_); + } +} + + +void GlonassL1CaPcpsAcquisition::set_doppler_max(unsigned int doppler_max) +{ + doppler_max_ = doppler_max; + + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_doppler_max(doppler_max_); + } + else + { + acquisition_cc_->set_doppler_max(doppler_max_); + } +} + + +void GlonassL1CaPcpsAcquisition::set_doppler_step(unsigned int doppler_step) +{ + doppler_step_ = doppler_step; + + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_doppler_step(doppler_step_); + } + else + { + acquisition_cc_->set_doppler_step(doppler_step_); + } +} + + +void GlonassL1CaPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro) +{ + gnss_synchro_ = gnss_synchro; + + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_gnss_synchro(gnss_synchro_); + } + else + { + acquisition_cc_->set_gnss_synchro(gnss_synchro_); + } +} + + +signed int GlonassL1CaPcpsAcquisition::mag() +{ + if (item_type_.compare("cshort") == 0) + { + return acquisition_sc_->mag(); + } + else + { + return acquisition_cc_->mag(); + } +} + + +void GlonassL1CaPcpsAcquisition::init() +{ + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->init(); + } + else + { + acquisition_cc_->init(); + } + + set_local_code(); +} + + +void GlonassL1CaPcpsAcquisition::set_local_code() +{ + + std::complex* code = new std::complex[code_length_]; + + glonass_l1_ca_code_gen_complex_sampled(code,/* gnss_synchro_->PRN,*/ fs_in_, 0); + + for (unsigned int i = 0; i < sampled_ms_; i++) + { + memcpy(&(code_[i*code_length_]), code, + sizeof(gr_complex)*code_length_); + } + + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_local_code(code_); + } + else + { + acquisition_cc_->set_local_code(code_); + } + + delete[] code; +} + + +void GlonassL1CaPcpsAcquisition::reset() +{ + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_active(true); + } + else + { + acquisition_cc_->set_active(true); + } +} + + +void GlonassL1CaPcpsAcquisition::set_state(int state) +{ + if (item_type_.compare("cshort") == 0) + { + acquisition_sc_->set_state(state); + } + else + { + acquisition_cc_->set_state(state); + } +} + + +float GlonassL1CaPcpsAcquisition::calculate_threshold(float pfa) +{ + //Calculate the threshold + unsigned int frequency_bins = 0; + /* + for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) + { + frequency_bins++; + } + */ + + frequency_bins = (2*doppler_max_ + doppler_step_)/doppler_step_; + + DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; + unsigned int ncells = vector_length_ * frequency_bins; + double exponent = 1 / static_cast(ncells); + double val = pow(1.0 - pfa, exponent); + double lambda = double(vector_length_); + boost::math::exponential_distribution mydist (lambda); + float threshold = (float)quantile(mydist,val); + + return threshold; +} + + +void GlonassL1CaPcpsAcquisition::connect(gr::top_block_sptr top_block) +{ + if (item_type_.compare("gr_complex") == 0) + { + top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0); + } + else if (item_type_.compare("cshort") == 0) + { + top_block->connect(stream_to_vector_, 0, acquisition_sc_, 0); + } + else if (item_type_.compare("cbyte") == 0) + { + top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); + top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); + top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0); + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + } +} + + +void GlonassL1CaPcpsAcquisition::disconnect(gr::top_block_sptr top_block) +{ + if (item_type_.compare("gr_complex") == 0) + { + top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0); + } + else if (item_type_.compare("cshort") == 0) + { + top_block->disconnect(stream_to_vector_, 0, acquisition_sc_, 0); + } + else if (item_type_.compare("cbyte") == 0) + { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex + top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); + top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); + top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0); + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + } +} + + +gr::basic_block_sptr GlonassL1CaPcpsAcquisition::get_left_block() +{ + if (item_type_.compare("gr_complex") == 0) + { + return stream_to_vector_; + } + else if (item_type_.compare("cshort") == 0) + { + return stream_to_vector_; + } + else if (item_type_.compare("cbyte") == 0) + { + return cbyte_to_float_x2_; + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + return nullptr; + } +} + + +gr::basic_block_sptr GlonassL1CaPcpsAcquisition::get_right_block() +{ + if (item_type_.compare("cshort") == 0) + { + return acquisition_sc_; + } + else + { + return acquisition_cc_; + } +} + diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h new file mode 100644 index 000000000..2cc43c90f --- /dev/null +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h @@ -0,0 +1,138 @@ + +#ifndef GNSS_SDR_GLONASS_L1_CA_PCPS_ACQUISITION_H_ +#define GNSS_SDR_GLONASS_L1_CA_PCPS_ACQUISITION_H_ + +#include +#include +#include +#include "gnss_synchro.h" +#include "acquisition_interface.h" +#include "pcps_acquisition_cc.h" +#include "pcps_acquisition_sc.h" +#include "complex_byte_to_float_x2.h" +#include + + + +class ConfigurationInterface; + +/*! + * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface + * for GPS L1 C/A signals + */ +class GlonassL1CaPcpsAcquisition: public AcquisitionInterface +{ +public: + GlonassL1CaPcpsAcquisition(ConfigurationInterface* configuration, + std::string role, unsigned int in_streams, + unsigned int out_streams); + + virtual ~GlonassL1CaPcpsAcquisition(); + + std::string role() + { + return role_; + } + + /*! + * \brief Returns "GLONASS_L1_CA_PCPS_Acquisition" + */ + std::string implementation() + { + return "GLONASS_L1_CA_PCPS_Acquisition"; + } + size_t item_size() + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + + /*! + * \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); + + /*! + * \brief Set acquisition channel unique ID + */ + void set_channel(unsigned int channel); + + /*! + * \brief Set statistics threshold of PCPS algorithm + */ + void set_threshold(float threshold); + + /*! + * \brief Set maximum Doppler off grid search + */ + void set_doppler_max(unsigned int doppler_max); + + /*! + * \brief Set Doppler steps for the grid search + */ + void set_doppler_step(unsigned int doppler_step); + + /*! + * \brief Initializes acquisition algorithm. + */ + void init(); + + /*! + * \brief Sets local code for GPS L1/CA PCPS acquisition algorithm. + */ + void set_local_code(); + + /*! + * \brief Returns the maximum peak of grid search + */ + signed int mag(); + + /*! + * \brief Restart acquisition algorithm + */ + void reset(); + + /*! + * \brief If state = 1, it forces the block to start acquiring from the first sample + */ + void set_state(int state); + +private: + ConfigurationInterface* configuration_; + pcps_acquisition_cc_sptr acquisition_cc_; + pcps_acquisition_sc_sptr acquisition_sc_; + gr::blocks::stream_to_vector::sptr stream_to_vector_; + gr::blocks::float_to_complex::sptr float_to_complex_; + complex_byte_to_float_x2_sptr cbyte_to_float_x2_; + size_t item_size_; + std::string item_type_; + unsigned int vector_length_; + unsigned int code_length_; + bool bit_transition_flag_; + bool use_CFAR_algorithm_flag_; + unsigned int channel_; + float threshold_; + unsigned int doppler_max_; + unsigned int doppler_step_; + unsigned int sampled_ms_; + unsigned int max_dwells_; + long fs_in_; + long if_; + bool dump_; + std::string dump_filename_; + std::complex * code_; + Gnss_Synchro * gnss_synchro_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; + + float calculate_threshold(float pfa); +}; + +#endif /* GNSS_SDR_GLONASS_L1_CA_PCPS_ACQUISITION_H_ */ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index 399f443b6..29506fbeb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -34,6 +34,7 @@ #include "pcps_acquisition_cc.h" #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include "control_message_factory.h" #include "GPS_L1_CA.h" //GPS_TWO_PI +#include "Glonass_L1_CA.h" //GLONASS_TWO_PI using google::LogMessage; @@ -76,6 +78,7 @@ pcps_acquisition_cc::pcps_acquisition_cc( d_active = false; d_state = 0; d_freq = freq; + d_old_freq = freq; d_fs_in = fs_in; d_samples_per_ms = samples_per_ms; d_samples_per_code = samples_per_code; @@ -158,6 +161,13 @@ pcps_acquisition_cc::~pcps_acquisition_cc() void pcps_acquisition_cc::set_local_code(std::complex * code) { + // reset the intermediate frequency + d_freq = d_old_freq; + // This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid + if( is_fdma() ) + { + update_grid_doppler_wipeoffs(); + } // COD // Here we want to create a buffer that looks like this: // [ 0 0 0 ... 0 c_0 c_1 ... c_L] @@ -179,6 +189,22 @@ void pcps_acquisition_cc::set_local_code(std::complex * code) } +bool pcps_acquisition_cc::is_fdma() +{ + // Dealing with FDMA system + if( strcmp(d_gnss_synchro->Signal,"1G") == 0 ) + { + d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); + LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN) << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; + return true; + } + else + { + return false; + } +} + + void pcps_acquisition_cc::update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq) { float phase_step_rad = GPS_TWO_PI * freq / static_cast(d_fs_in); @@ -215,6 +241,20 @@ void pcps_acquisition_cc::init() } +void pcps_acquisition_cc::update_grid_doppler_wipeoffs() +{ + // Create the carrier Doppler wipeoff signals + d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; + + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) + { + d_grid_doppler_wipeoffs[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + int doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; + update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_freq + doppler); + } +} + + void pcps_acquisition_cc::set_state(int state) { gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 07fcb32a9..4bd5ab857 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -56,6 +56,7 @@ #include #include #include "gnss_synchro.h" +#include "Glonass_L1_CA.h" //GLONASS_TWO_PI class pcps_acquisition_cc; @@ -94,11 +95,14 @@ private: std::string dump_filename); void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); + void update_grid_doppler_wipeoffs(); + bool is_fdma(); void send_negative_acquisition(); void send_positive_acquisition(); long d_fs_in; long d_freq; + long d_old_freq; int d_samples_per_ms; int d_samples_per_code; //unsigned int d_doppler_resolution; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc index c265dee17..7200ce946 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc @@ -41,6 +41,8 @@ #include #include "control_message_factory.h" #include "GPS_L1_CA.h" //GPS_TWO_PI +#include "Glonass_L1_CA.h" //GLONASS_TWO_PI + using google::LogMessage; @@ -74,6 +76,7 @@ pcps_acquisition_sc::pcps_acquisition_sc( d_active = false; d_state = 0; d_freq = freq; + d_old_freq = freq; d_fs_in = fs_in; d_samples_per_ms = samples_per_ms; d_samples_per_code = samples_per_code; @@ -159,6 +162,13 @@ pcps_acquisition_sc::~pcps_acquisition_sc() void pcps_acquisition_sc::set_local_code(std::complex * code) { + // reset the intermediate frequency + d_freq = d_old_freq; + // This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid + if( is_fdma() ) + { + update_grid_doppler_wipeoffs(); + } // COD // Here we want to create a buffer that looks like this: // [ 0 0 0 ... 0 c_0 c_1 ... c_L] @@ -175,6 +185,22 @@ void pcps_acquisition_sc::set_local_code(std::complex * code) } +bool pcps_acquisition_sc::is_fdma() +{ + // Dealing with FDMA system + if( strcmp(d_gnss_synchro->Signal,"1G") == 0 ) + { + d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); + LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN) << " in Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; + return true; + } + else + { + return false; + } +} + + void pcps_acquisition_sc::update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq) { float phase_step_rad = GPS_TWO_PI * freq / static_cast(d_fs_in); @@ -211,6 +237,19 @@ void pcps_acquisition_sc::init() } +void pcps_acquisition_sc::update_grid_doppler_wipeoffs() +{ + // Create the carrier Doppler wipeoff signals + d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; + + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) + { + d_grid_doppler_wipeoffs[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + int doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; + update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_freq + doppler); + } +} + void pcps_acquisition_sc::set_state(int state) { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.h index 1b3ab7bf6..093c88dd8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.h @@ -97,8 +97,12 @@ private: int correlator_length_samples, float freq); + void update_grid_doppler_wipeoffs(); + bool is_fdma(); + long d_fs_in; long d_freq; + long d_old_freq; int d_samples_per_ms; int d_samples_per_code; //unsigned int d_doppler_resolution; diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index e7d8b60bd..818c9d947 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -25,6 +25,7 @@ set(GNSS_SPLIBS_SOURCES gnss_sdr_sample_counter.cc gnss_signal_processing.cc gps_sdr_signal_processing.cc + glonass_l1_signal_processing.cc pass_through.cc galileo_e5_signal_processing.cc complex_byte_to_float_x2.cc diff --git a/src/algorithms/libs/glonass_l1_signal_processing.cc b/src/algorithms/libs/glonass_l1_signal_processing.cc new file mode 100644 index 000000000..3ee50707a --- /dev/null +++ b/src/algorithms/libs/glonass_l1_signal_processing.cc @@ -0,0 +1,153 @@ +/*! + * \file glonass_l1_signal_processing.cc + * \brief This class implements various functions for GLONASS L1 CA signals + * \author Javier Arribas, 2011. jarribas(at)cttc.es + * + * Detailed description of the file here if needed. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_l1_signal_processing.h" + +auto auxCeil = [](float x){ return static_cast(static_cast((x)+1)); }; + +void glonass_l1_ca_code_gen_complex(std::complex* _dest,/* signed int _prn,*/ unsigned int _chip_shift) +{ + const unsigned int _code_length = 511; + bool G1[_code_length]; + bool G1_register[9]; + bool feedback1; + bool aux; + unsigned int delay; + unsigned int lcv, lcv2; + + for(lcv = 0; lcv < 9; lcv++) + { + G1_register[lcv] = 1; + } + + /* Generate G1 Register */ + for(lcv = 0; lcv < _code_length; lcv++) + { + G1[lcv] = G1_register[2]; + + feedback1 = G1_register[4]^G1_register[0]; + + for(lcv2 = 0; lcv2 < 8; lcv2++) + { + G1_register[lcv2] = G1_register[lcv2 + 1]; + } + + G1_register[8] = feedback1; + } + + /* Generate PRN from G1 Register */ + for(lcv = 0; lcv < _code_length; lcv++) + { + aux = G1[lcv]; + if(aux == true) + { + _dest[lcv] = std::complex(1, 0); + } + else + { + _dest[lcv] = std::complex(-1, 0); + } + } + + /* Set the delay */ + delay = _code_length; + delay += _chip_shift; + delay %= _code_length; + + /* Generate PRN from G1 and G2 Registers */ + for(lcv = 0; lcv < _code_length; lcv++) + { + aux = G1[(lcv + _chip_shift) % _code_length]; + if(aux == true) + { + _dest[lcv] = std::complex(1, 0); + } + else + { + _dest[lcv] = std::complex(-1, 0); + } + delay++; + delay %= _code_length; + } +} + + +/* + * Generates complex GLONASS L1 C/A code for the desired SV ID and sampled to specific sampling frequency + */ +void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift) +{ + // This function is based on the GNU software GPS for MATLAB in the Kay Borre book + std::complex _code[511]; + signed int _samplesPerCode, _codeValueIndex; + float _ts; + float _tc; + float aux; + const signed int _codeFreqBasis = 511000; //Hz + const signed int _codeLength = 511; + + //--- Find number of samples per spreading code ---------------------------- + _samplesPerCode = static_cast(static_cast(_fs) / static_cast(_codeFreqBasis / _codeLength)); + + //--- Find time constants -------------------------------------------------- + _ts = 1.0 / static_cast(_fs); // Sampling period in sec + _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec + glonass_l1_ca_code_gen_complex(_code, _chip_shift); //generate C/A code 1 sample per chip + + for (signed int i = 0; i < _samplesPerCode; i++) + { + //=== Digitizing ======================================================= + + //--- Make index array to read C/A code values ------------------------- + // The length of the index array depends on the sampling frequency - + // number of samples per millisecond (because one C/A code period is one + // millisecond). + + // _codeValueIndex = ceil((_ts * ((float)i + 1)) / _tc) - 1; + aux = (_ts * (i + 1)) / _tc; + _codeValueIndex = auxCeil( aux ) - 1; + + //--- Make the digitized version of the C/A code ----------------------- + // The "upsampled" code is made by selecting values form the CA code + // chip array (caCode) for the time instances of each sample. + if (i == _samplesPerCode - 1) + { + //--- Correct the last index (due to number rounding issues) ----------- + _dest[i] = _code[_codeLength - 1]; + + } + else + { + _dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample + } + } +} diff --git a/src/algorithms/libs/glonass_l1_signal_processing.h b/src/algorithms/libs/glonass_l1_signal_processing.h new file mode 100644 index 000000000..891febcb9 --- /dev/null +++ b/src/algorithms/libs/glonass_l1_signal_processing.h @@ -0,0 +1,47 @@ +/*! + * \file glonass_l1_signal_processing.h + * \brief This class implements various functions for GLONASS L1 CA signals + * \author Gabriel Araujo, 2017. gabriel.araujo(at)ieee.org + * + * Detailed description of the file here if needed. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GLONASS_SDR_SIGNAL_PROCESSING_H_ +#define GNSS_SDR_GLONASS_SDR_SIGNAL_PROCESSING_H_ + +#include + +//!Generates complex GLONASS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency +void glonass_l1_ca_code_gen_complex(std::complex* _dest, /*signed int _prn,*/ unsigned int _chip_shift); + +//! Generates N complex GLONASS L1 C/A codes for the desired SV ID and code shift +void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift, unsigned int _ncodes); + +//! Generates complex GLONASS L1 C/A code for the desired SV ID and code shift +void glonass_l1_ca_code_gen_complex_sampled(std::complex* _dest,/* unsigned int _prn,*/ signed int _fs, unsigned int _chip_shift); + +#endif /* GNSS_SDR_GLONASS_SDR_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/signal_generator/adapters/signal_generator.cc b/src/algorithms/signal_generator/adapters/signal_generator.cc index 35ecfdddc..a2ef90cdf 100644 --- a/src/algorithms/signal_generator/adapters/signal_generator.cc +++ b/src/algorithms/signal_generator/adapters/signal_generator.cc @@ -36,6 +36,7 @@ #include "Galileo_E1.h" #include "GPS_L1_CA.h" #include "Galileo_E5a.h" +#include "Glonass_L1_CA.h" using google::LogMessage; @@ -102,6 +103,11 @@ SignalGenerator::SignalGenerator(ConfigurationInterface* configuration, vector_length = round(static_cast(fs_in) / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); } + else if (std::find(system.begin(), system.end(), "R") != system.end()) + { + vector_length = round((float)fs_in + / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); + } if (item_type_.compare("gr_complex") == 0) { diff --git a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc index 86f488588..a99c4751d 100644 --- a/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc +++ b/src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc @@ -34,11 +34,13 @@ #include #include #include "gps_sdr_signal_processing.h" +#include "glonass_l1_signal_processing.h" #include "galileo_e1_signal_processing.h" #include "galileo_e5_signal_processing.h" #include "Galileo_E1.h" #include "Galileo_E5a.h" #include "GPS_L1_CA.h" +#include "Glonass_L1_CA.h" /* * Create a new instance of signal_generator_c and return @@ -107,6 +109,14 @@ void signal_generator_c::init() num_of_codes_per_vector_.push_back(galileo_signal ? 4 * static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH) : 1); data_bit_duration_ms_.push_back(1e3 / GPS_CA_TELEMETRY_RATE_BITS_SECOND); } + else if (system_[sat] == "R") + { + samples_per_code_.push_back(round(static_cast(fs_in_) + / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS))); + + num_of_codes_per_vector_.push_back(galileo_signal ? 4 * static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH) : 1); + data_bit_duration_ms_.push_back(1e3 / GLONASS_CA_TELEMETRY_RATE_BITS_SECOND); + } else if (system_[sat] == "E") { if (signal_[sat].at(0) == '5') @@ -160,6 +170,28 @@ void signal_generator_c::generate_codes() } } + // Concatenate "num_of_codes_per_vector_" codes + for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++) + { + memcpy(&(sampled_code_data_[sat][i * samples_per_code_[sat]]), + code, sizeof(gr_complex) * samples_per_code_[sat]); + } + } + else if (system_[sat] == "R") + { + // Generate one code-period of 1G signal + glonass_l1_ca_code_gen_complex_sampled(code, /*PRN_[sat],*/ fs_in_, + static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS) - delay_chips_[sat]); + + // Obtain the desired CN0 assuming that Pn = 1. + if (noise_flag_) + { + for (unsigned int i = 0; i < samples_per_code_[sat]; i++) + { + code[i] *= sqrt(pow(10,CN0_dB_[sat] / 10) / BW_BB_); + } + } + // Concatenate "num_of_codes_per_vector_" codes for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++) { @@ -261,6 +293,8 @@ int signal_generator_c::general_work (int noutput_items __attribute__((unused)), unsigned int out_idx = 0; unsigned int i = 0; unsigned int k = 0; + // the intermediate frequency must be set by the user + unsigned int freq = 4e6; for (out_idx = 0; out_idx < vector_length_; out_idx++) { @@ -311,6 +345,45 @@ int signal_generator_c::general_work (int noutput_items __attribute__((unused)), } } + else if (system_[sat] == "R") + { + phase_step_rad = -static_cast(GPS_TWO_PI) * (freq + (DFRQ1_GLO * GLONASS_PRN.at(PRN_[sat])) + doppler_Hz_[sat]) / static_cast(fs_in_); + // std::cout << "sat " << PRN_[sat] << " SG - Freq = " << (freq + (DFRQ1_GLO * GLONASS_PRN.at(PRN_[sat]))) << " Doppler = " << doppler_Hz_[sat] << std::endl; + _phase[0] = -start_phase_rad_[sat]; + volk_gnsssdr_s32f_sincos_32fc(complex_phase_, -phase_step_rad, _phase, vector_length_); + + unsigned int delay_samples = (delay_chips_[sat] % static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS)) + * samples_per_code_[sat] / GLONASS_L1_CA_CODE_LENGTH_CHIPS; + + for (i = 0; i < num_of_codes_per_vector_[sat]; i++) + { + for (k = 0; k < delay_samples; k++) + { + out[out_idx] += sampled_code_data_[sat][out_idx] + * current_data_bits_[sat] + * complex_phase_[out_idx]; + out_idx++; + } + + if (ms_counter_[sat] == 0 && data_flag_) + { + // New random data bit + current_data_bits_[sat] = gr_complex((uniform_dist(e1) % 2) == 0 ? 1 : -1, 0); + } + + for (k = delay_samples; k < samples_per_code_[sat]; k++) + { + out[out_idx] += sampled_code_data_[sat][out_idx] + * current_data_bits_[sat] + * complex_phase_[out_idx]; + out_idx++; + } + + ms_counter_[sat] = (ms_counter_[sat] + static_cast(round(1e3*GLONASS_L1_CA_CODE_PERIOD))) + % data_bit_duration_ms_[sat]; + } + } + else if (system_[sat] == "E") { if(signal_[sat].at(0)=='5') diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index da2051114..0502aac5b 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -33,6 +33,8 @@ set(TRACKING_ADAPTER_SOURCES gps_l1_ca_tcp_connector_tracking.cc galileo_e5a_dll_pll_tracking.cc gps_l2_m_dll_pll_tracking.cc + glonass_l1_ca_dll_pll_tracking.cc + glonass_l1_ca_dll_pll_c_aid_tracking.cc ${OPT_TRACKING_ADAPTERS} ) diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc new file mode 100644 index 000000000..6f9bf340a --- /dev/null +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc @@ -0,0 +1,191 @@ +#include "glonass_l1_ca_dll_pll_c_aid_tracking.h" +#include +#include "Glonass_L1_CA.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +GlonassL1CaDllPllCAidTracking::GlonassL1CaDllPllCAidTracking( + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## + int fs_in; + int vector_length; + int f_if; + bool dump; + std::string dump_filename; + std::string default_item_type = "gr_complex"; + float pll_bw_hz; + float pll_bw_narrow_hz; + float dll_bw_hz; + float dll_bw_narrow_hz; + float early_late_space_chips; + item_type_ = configuration->property(role + ".item_type", default_item_type); + //vector_length = configuration->property(role + ".vector_length", 2048); + fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); + f_if = configuration->property(role + ".if", 0); + dump = configuration->property(role + ".dump", false); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); + dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); + int extend_correlation_ms; + extend_correlation_ms = configuration->property(role + ".extend_correlation_ms", 1); + + early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + std::string default_dump_filename = "./track_ch"; + dump_filename = configuration->property(role + ".dump_filename", + default_dump_filename); //unused! + vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); + + //################# MAKE TRACKING GNURadio object ################### + if (item_type_.compare("gr_complex") == 0) + { + item_size_ = sizeof(gr_complex); + tracking_cc = glonass_l1_ca_dll_pll_c_aid_make_tracking_cc( + f_if, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + extend_correlation_ms, + early_late_space_chips); + DLOG(INFO) << "tracking(" << tracking_cc->unique_id() << ")"; + } + else if(item_type_.compare("cshort") == 0) + { + item_size_ = sizeof(lv_16sc_t); + tracking_sc = glonass_l1_ca_dll_pll_c_aid_make_tracking_sc( + f_if, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + extend_correlation_ms, + early_late_space_chips); + DLOG(INFO) << "tracking(" << tracking_sc->unique_id() << ")"; + } + else + { + item_size_ = sizeof(gr_complex); + LOG(WARNING) << item_type_ << " unknown tracking item type."; + } + channel_ = 0; +} + + +GlonassL1CaDllPllCAidTracking::~GlonassL1CaDllPllCAidTracking() +{} + + +void GlonassL1CaDllPllCAidTracking::start_tracking() +{ + + if (item_type_.compare("gr_complex") == 0) + { + tracking_cc->start_tracking(); + } + else if (item_type_.compare("cshort") == 0) + { + tracking_sc->start_tracking(); + } + else + { + LOG(WARNING) << item_type_ << " unknown tracking item type"; + } +} + +/* + * Set tracking channel unique ID + */ +void GlonassL1CaDllPllCAidTracking::set_channel(unsigned int channel) +{ + channel_ = channel; + + if (item_type_.compare("gr_complex") == 0) + { + tracking_cc->set_channel(channel); + } + else if (item_type_.compare("cshort") == 0) + { + tracking_sc->set_channel(channel); + } + else + { + LOG(WARNING) << item_type_ << " unknown tracking item type"; + } +} + +void GlonassL1CaDllPllCAidTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + if (item_type_.compare("gr_complex") == 0) + { + tracking_cc->set_gnss_synchro(p_gnss_synchro); + } + else if (item_type_.compare("cshort") == 0) + { + tracking_sc->set_gnss_synchro(p_gnss_synchro); + } + else + { + LOG(WARNING) << item_type_ << " unknown tracking item type"; + } +} + +void GlonassL1CaDllPllCAidTracking::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 GlonassL1CaDllPllCAidTracking::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 GlonassL1CaDllPllCAidTracking::get_left_block() +{ + if (item_type_.compare("gr_complex") == 0) + { + return tracking_cc; + } + else if (item_type_.compare("cshort") == 0) + { + return tracking_sc; + } + else + { + LOG(WARNING) << item_type_ << " unknown tracking item type"; + return nullptr; + } +} + +gr::basic_block_sptr GlonassL1CaDllPllCAidTracking::get_right_block() +{ + if (item_type_.compare("gr_complex") == 0) + { + return tracking_cc; + } + else if (item_type_.compare("cshort") == 0) + { + return tracking_sc; + } + else + { + LOG(WARNING) << item_type_ << " unknown tracking item type"; + return nullptr; + } +} diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h new file mode 100644 index 000000000..19a9502b5 --- /dev/null +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.h @@ -0,0 +1,72 @@ +#ifndef GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_H_ +#define GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_H_ + +#include +#include "tracking_interface.h" +#include "glonass_l1_ca_dll_pll_c_aid_tracking_cc.h" +#include "glonass_l1_ca_dll_pll_c_aid_tracking_sc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a code DLL + carrier PLL tracking loop + */ +class GlonassL1CaDllPllCAidTracking : public TrackingInterface +{ +public: + + GlonassL1CaDllPllCAidTracking(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GlonassL1CaDllPllCAidTracking(); + + std::string role() + { + return role_; + } + + //! Returns "GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking" + std::string implementation() + { + return "GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking"; + } + size_t item_size() + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + + + /*! + * \brief Set tracking channel unique ID + */ + void set_channel(unsigned int channel); + + /*! + * \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); + + + void start_tracking(); + +private: + glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr tracking_cc; + glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr tracking_sc; + size_t item_size_; + std::string item_type_; + unsigned int channel_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; +}; + +#endif // GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_H_ diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc new file mode 100644 index 000000000..cce1b19a7 --- /dev/null +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc @@ -0,0 +1,148 @@ +/*! + * \file glonass_l1_ca_dll_pll_tracking.cc + * \brief Interface of an adapter of a DLL+PLL tracking loop block + * for Glonass L1 C/A to a TrackingInterface + * \author Gabriel Araujo, 2017. gabriel.araujo.5000(at)gmail.com + * \author Luis Esteve, 2017. 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, Birkha user, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_l1_ca_dll_pll_tracking.h" +#include +#include "Glonass_L1_CA.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +GlonassL1CaDllPllTracking::GlonassL1CaDllPllTracking( + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + DLOG(INFO) << "role " << role; + //################# CONFIGURATION PARAMETERS ######################## + int fs_in; + int vector_length; + int f_if; + bool dump; + std::string dump_filename; + std::string item_type; + std::string default_item_type = "gr_complex"; + float pll_bw_hz; + float dll_bw_hz; + float early_late_space_chips; + item_type = configuration->property(role + ".item_type", default_item_type); + fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); + f_if = configuration->property(role + ".if", 0); + dump = configuration->property(role + ".dump", false); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + std::string default_dump_filename = "./track_ch"; + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); + + //################# MAKE TRACKING GNURadio object ################### + if (item_type.compare("gr_complex") == 0) + { + item_size_ = sizeof(gr_complex); + tracking_ = glonass_l1_ca_dll_pll_make_tracking_cc( + f_if, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + early_late_space_chips); + } + else + { + item_size_ = sizeof(gr_complex); + LOG(WARNING) << item_type << " unknown tracking item type."; + } + channel_ = 0; + DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; +} + + +GlonassL1CaDllPllTracking::~GlonassL1CaDllPllTracking() +{} + + +void GlonassL1CaDllPllTracking::start_tracking() +{ + tracking_->start_tracking(); +} + + +/* + * Set tracking channel unique ID + */ +void GlonassL1CaDllPllTracking::set_channel(unsigned int channel) +{ + channel_ = channel; + tracking_->set_channel(channel); +} + + +void GlonassL1CaDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + tracking_->set_gnss_synchro(p_gnss_synchro); +} + + +void GlonassL1CaDllPllTracking::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 GlonassL1CaDllPllTracking::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 GlonassL1CaDllPllTracking::get_left_block() +{ + return tracking_; +} + + +gr::basic_block_sptr GlonassL1CaDllPllTracking::get_right_block() +{ + return tracking_; +} diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h new file mode 100644 index 000000000..46565b2e1 --- /dev/null +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.h @@ -0,0 +1,105 @@ +/*! + * \file glonass_l1_ca_dll_pll_tracking.h + * \brief Interface of an adapter of a DLL+PLL tracking loop block + * for Glonass L1 C/A to a TrackingInterface + * \author Gabriel Araujo, 2017. gabriel.araujo.5000(at)gmail.com + * \author Luis Esteve, 2017. 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, Birkha user, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_H_ +#define GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_H_ + +#include +#include "tracking_interface.h" +#include "glonass_l1_ca_dll_pll_tracking_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a code DLL + carrier PLL tracking loop + */ +class GlonassL1CaDllPllTracking : public TrackingInterface +{ +public: + GlonassL1CaDllPllTracking(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GlonassL1CaDllPllTracking(); + + std::string role() + { + return role_; + } + + //! Returns "GLONASS_L1_CA_DLL_PLL_Tracking" + std::string implementation() + { + return "GLONASS_L1_CA_DLL_PLL_Tracking"; + } + + size_t item_size() + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + + /*! + * \brief Set tracking channel unique ID + */ + void set_channel(unsigned int channel); + + /*! + * \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); + + void start_tracking(); + +private: + glonass_l1_ca_dll_pll_tracking_cc_sptr tracking_; + size_t item_size_; + unsigned int channel_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; +}; + +#endif // GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index d7c8981c6..97e609a5f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -34,7 +34,10 @@ set(TRACKING_GR_BLOCKS_SOURCES galileo_e5a_dll_pll_tracking_cc.cc gps_l2_m_dll_pll_tracking_cc.cc gps_l1_ca_dll_pll_c_aid_tracking_cc.cc - gps_l1_ca_dll_pll_c_aid_tracking_sc.cc + gps_l1_ca_dll_pll_c_aid_tracking_sc.cc + glonass_l1_ca_dll_pll_tracking_cc.cc + glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc + glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc ${OPT_TRACKING_BLOCKS} ) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc new file mode 100644 index 000000000..231058ec8 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -0,0 +1,646 @@ +#include "glonass_l1_ca_dll_pll_c_aid_tracking_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "glonass_l1_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "Glonass_L1_CA.h" +#include "control_message_factory.h" + + +/*! + * \todo Include in definition header file + */ +#define CN0_ESTIMATION_SAMPLES 20 +#define MINIMUM_VALID_CN0 25 +#define MAXIMUM_LOCK_FAIL_COUNTER 50 +#define CARRIER_LOCK_THRESHOLD 0.85 + + +using google::LogMessage; + +glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr +glonass_l1_ca_dll_pll_c_aid_make_tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips) +{ + return glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr(new glonass_l1_ca_dll_pll_c_aid_tracking_cc(if_freq, + fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz,pll_bw_narrow_hz, dll_bw_narrow_hz, extend_correlation_ms, early_late_space_chips)); +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_cc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(pmt::pmt_t msg) +{ + //pmt::print(msg); + DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); + if (d_enable_extended_integration == false) //avoid re-setting preamble indicator + { + d_preamble_timestamp_s = pmt::to_double(msg); + d_enable_extended_integration = true; + d_preamble_synchronized = false; + } +} + + +glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips) : + gr::block("glonass_l1_ca_dll_pll_c_aid_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + + this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + boost::bind(&glonass_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index, this, _1)); + + this->message_port_register_out(pmt::mp("events")); + // initialize internal vars + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + d_correlation_length_samples = static_cast(d_vector_length); + + // Initialize tracking ========================================== + d_pll_bw_hz = pll_bw_hz; + d_dll_bw_hz = dll_bw_hz; + d_pll_bw_narrow_hz = pll_bw_narrow_hz; + d_dll_bw_narrow_hz = dll_bw_narrow_hz; + d_extend_correlation_ms = extend_correlation_ms; + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz, 2); + + // --- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the C/A code replica sampled 1x/chip + d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, and Late + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); + + //--- Perform initializations ------------------------------ + // define initial code frequency basis of NCO + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carrier_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; //(from trk to tlm) + d_acq_sample_stamp = 0; + d_enable_tracking = false; + d_pull_in = false; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + + systemName["R"] = std::string("Glonass"); + + set_relative_rate(1.0 / static_cast(d_vector_length)); + + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_code_error_filt_chips_Ti = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_code_phase_samples = 0.0; + + d_pll_to_dll_assist_secs_Ti = 0.0; + d_rem_code_phase_chips = 0.0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_enable_extended_integration = false; + d_preamble_synchronized = false; + d_rem_code_phase_integer_samples = 0; + d_code_error_chips_Ti = 0.0; + d_code_error_filt_chips_s = 0.0; + d_carr_phase_error_secs_Ti = 0.0; + d_preamble_timestamp_s = 0.0; + //set_min_output_buffer((long int)300); +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() +{ + /* + * correct the code phase according to the delay between acq and trk + */ + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1.0 / d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_correlation_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + if (corrected_acq_phase_samples < 0) + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } + delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + glonass_l1_ca_code_gen_complex(d_ca_code, 0); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carrier_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_pll_to_dll_assist_secs_Ti = 0.0; + d_code_phase_samples = d_acq_code_phase_samples; + + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + + // DEBUG OUTPUT + std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking + d_pull_in = true; + d_enable_tracking = true; + d_enable_extended_integration = true; + d_preamble_synchronized = true; + LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; +} + + +glonass_l1_ca_dll_pll_c_aid_tracking_cc::~glonass_l1_ca_dll_pll_c_aid_tracking_cc() +{ + d_dump_file.close(); + + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_ca_code); + + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); +} + + + +int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // Block input data and block output stream pointers + const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + // process vars + double code_error_filt_secs_Ti = 0.0; + double CURRENT_INTEGRATION_TIME_S = 0.0; + double CORRECTED_INTEGRATION_TIME_S = 0.0; + + if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) + { + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter += samples_offset; // count for the processed samples + d_pull_in = false; + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs=d_fs_in; + *out[0] = current_synchro_data; + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + + // ####### coherent intergration extension + // keep the last symbols + d_E_history.push_back(d_correlator_outs[0]); // save early output + d_P_history.push_back(d_correlator_outs[1]); // save prompt output + d_L_history.push_back(d_correlator_outs[2]); // save late output + + if (static_cast(d_P_history.size()) > d_extend_correlation_ms) + { + d_E_history.pop_front(); + d_P_history.pop_front(); + d_L_history.pop_front(); + } + + bool enable_dll_pll; + if (d_enable_extended_integration == true) + { + long int symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) + { + // compute coherent integration and enable tracking loop + // perform coherent integration using correlator output history + // std::cout<<"##### RESET COHERENT INTEGRATION ####"<PRN) + << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl + << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; + } + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_extend_correlation_ms) * GLONASS_L1_CA_CODE_PERIOD; + d_code_loop_filter.set_pdi(CURRENT_INTEGRATION_TIME_S); + enable_dll_pll = true; + } + else + { + if(d_preamble_synchronized == true) + { + // continue extended coherent correlation + // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + int K_prn_samples = round(T_prn_samples); + double K_T_prn_error_samples = K_prn_samples - T_prn_samples; + + d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples; + d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples + d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; + d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast(d_correlation_length_samples), GLONASS_TWO_PI); + + // UPDATE ACCUMULATED CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI; + + // disable tracking loop and inform telemetry decoder + enable_dll_pll = false; + } + else + { + // perform basic (1ms) correlation + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + d_code_loop_filter.set_pdi(CURRENT_INTEGRATION_TIME_S); + enable_dll_pll = true; + } + } + } + else + { + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + enable_dll_pll = true; + } + + if (enable_dll_pll == true) + { + // ################## PLL ########################################################## + // Update PLL discriminator [rads/Ti -> Secs/Ti] + d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output + // Carrier discriminator filter + // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan + // Input [s/Ti] -> output [Hz] + d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S); + // PLL to DLL assistance [Secs/Ti] + d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch; + // code Doppler frequency update + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); + + // ################## DLL ########################################################## + // DLL discriminator + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + // Code discriminator filter + d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] + d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; + code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_prn_samples = round(T_prn_samples); + double K_T_prn_error_samples = K_prn_samples - T_prn_samples; + + d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast(d_fs_in); + d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples + d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; + d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; + + //################### PLL COMMANDS ################################################# + //carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI; + // UPDATE ACCUMULATED CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); + //remnant carrier phase [rad] + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI); + + //################### DLL COMMANDS ################################################# + //code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ####################################### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; // prompt + d_cn0_estimation_counter++; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GLONASS_L1_CA_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + if (d_preamble_synchronized == true) + { + current_synchro_data.correlation_length_ms = d_extend_correlation_ms; + } + else + { + current_synchro_data.correlation_length_ms = 1; + } + } + else + { + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + } + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + current_synchro_data.System = {'R'}; + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + } + //assign the GNURadio block output data + current_synchro_data.fs=d_fs_in; + *out[0] = current_synchro_data; + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + //PLL commands + d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + + //DLL commands + d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "Exception writing trk dump file " << e->what(); + } + } + + consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_correlation_length_samples; //count for the processed samples + + return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int channel) +{ + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl; + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl; + } + } + } +} + + + +void glonass_l1_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h new file mode 100644 index 000000000..280d590b1 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h @@ -0,0 +1,167 @@ +#ifndef GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H +#define GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H + +#include +#include +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_FLL_PLL_filter.h" +//#include "tracking_loop_filter.h" +#include "cpu_multicorrelator.h" + +class glonass_l1_ca_dll_pll_c_aid_tracking_cc; + +typedef boost::shared_ptr + glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr; + +glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr +glonass_l1_ca_dll_pll_c_aid_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class glonass_l1_ca_dll_pll_c_aid_tracking_cc: public gr::block +{ +public: + ~glonass_l1_ca_dll_pll_c_aid_tracking_cc(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_tracking(); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + +private: + friend glonass_l1_ca_dll_pll_c_aid_tracking_cc_sptr + glonass_l1_ca_dll_pll_c_aid_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + glonass_l1_ca_dll_pll_c_aid_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + // tracking configuration vars + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + + long d_if_freq; + long d_fs_in; + long d_glonass_freq_ch; + + double d_early_late_spc_chips; + int d_n_correlator_taps; + + gr_complex* d_ca_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_code_phase_chips; + double d_rem_carrier_phase_rad; + int d_rem_code_phase_integer_samples; + + // PLL and DLL filter library + //Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_2nd_DLL_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 + float d_dll_bw_hz; + float d_pll_bw_hz; + float d_dll_bw_narrow_hz; + float d_pll_bw_narrow_hz; + double d_code_freq_chips; + double d_code_phase_step_chips; + double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; + double d_acc_carrier_phase_cycles; + double d_code_phase_samples; + double d_pll_to_dll_assist_secs_Ti; + double d_code_error_chips_Ti; + double d_code_error_filt_chips_s; + double d_code_error_filt_chips_Ti; + double d_carr_phase_error_secs_Ti; + + // symbol history to detect bit transition + std::deque d_E_history; + std::deque d_P_history; + std::deque d_L_history; + double d_preamble_timestamp_s; + int d_extend_correlation_ms; + bool d_enable_extended_integration; + bool d_preamble_synchronized; + void msg_handler_preamble_index(pmt::pmt_t msg); + + //Integration period in samples + int d_correlation_length_samples; + + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + gr_complex* d_Prompt_buffer; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + int d_carrier_lock_fail_counter; + + // control vars + bool d_enable_tracking; + bool d_pull_in; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc new file mode 100644 index 000000000..f22bbc732 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -0,0 +1,648 @@ +#include "glonass_l1_ca_dll_pll_c_aid_tracking_sc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "glonass_l1_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "Glonass_L1_CA.h" +#include "control_message_factory.h" + + +/*! + * \todo Include in definition header file + */ +#define CN0_ESTIMATION_SAMPLES 20 +#define MINIMUM_VALID_CN0 25 +#define MAXIMUM_LOCK_FAIL_COUNTER 50 +#define CARRIER_LOCK_THRESHOLD 0.85 + + +using google::LogMessage; + +glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr +glonass_l1_ca_dll_pll_c_aid_make_tracking_sc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips) +{ + return glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr(new glonass_l1_ca_dll_pll_c_aid_tracking_sc(if_freq, + fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, pll_bw_narrow_hz, dll_bw_narrow_hz, extend_correlation_ms, early_late_space_chips)); +} + + + +void glonass_l1_ca_dll_pll_c_aid_tracking_sc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index(pmt::pmt_t msg) +{ + //pmt::print(msg); + DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); + if (d_enable_extended_integration == false) //avoid re-setting preamble indicator + { + d_preamble_timestamp_s = pmt::to_double(msg); + d_enable_extended_integration = true; + d_preamble_synchronized = false; + } +} + +glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips) : + gr::block("glonass_l1_ca_dll_pll_c_aid_tracking_sc", gr::io_signature::make(1, 1, sizeof(lv_16sc_t)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + boost::bind(&glonass_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index, this, _1)); + this->message_port_register_out(pmt::mp("events")); + // initialize internal vars + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + d_correlation_length_samples = static_cast(d_vector_length); + + // Initialize tracking ========================================== + d_pll_bw_hz = pll_bw_hz; + d_dll_bw_hz = dll_bw_hz; + d_pll_bw_narrow_hz = pll_bw_narrow_hz; + d_dll_bw_narrow_hz = dll_bw_narrow_hz; + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz, 2); + d_extend_correlation_ms = extend_correlation_ms; + + // --- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the C/A code replica sampled 1x/chip + d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_ca_code_16sc = static_cast(volk_gnsssdr_malloc(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, and Late + + d_correlator_outs_16sc = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_cmake(0,0); + } + + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu_16sc.init(2 * d_correlation_length_samples, d_n_correlator_taps); + + //--- Perform initializations ------------------------------ + // define initial code frequency basis of NCO + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carrier_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; //(from trk to tlm) + d_acq_sample_stamp = 0; + d_enable_tracking = false; + d_pull_in = false; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + + systemName["R"] = std::string("Glonass"); + + set_relative_rate(1.0 / static_cast(d_vector_length)); + + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_code_phase_samples = 0.0; + d_enable_extended_integration = false; + d_preamble_synchronized = false; + d_rem_code_phase_integer_samples = 0; + d_code_error_chips_Ti = 0.0; + d_pll_to_dll_assist_secs_Ti = 0.0; + d_rem_code_phase_chips = 0.0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_code_error_filt_chips_s = 0.0; + d_code_error_filt_chips_Ti = 0.0; + d_preamble_timestamp_s = 0.0; + d_carr_phase_error_secs_Ti = 0.0; + //set_min_output_buffer((long int)300); +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() +{ + /* + * correct the code phase according to the delay between acq and trk + */ + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1.0 / d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_correlation_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + if (corrected_acq_phase_samples < 0) + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } + delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + glonass_l1_ca_code_gen_complex(d_ca_code, 0); + volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS)); + + multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_16sc_t(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carrier_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_cycles = 0.0; + d_pll_to_dll_assist_secs_Ti = 0.0; + d_code_phase_samples = d_acq_code_phase_samples; + + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + + // DEBUG OUTPUT + std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking + d_pull_in = true; + d_enable_tracking = true; + d_enable_extended_integration = true; + d_preamble_synchronized = true; + + LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; +} + + +glonass_l1_ca_dll_pll_c_aid_tracking_sc::~glonass_l1_ca_dll_pll_c_aid_tracking_sc() +{ + d_dump_file.close(); + + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_ca_code); + volk_gnsssdr_free(d_ca_code_16sc); + volk_gnsssdr_free(d_correlator_outs_16sc); + + delete[] d_Prompt_buffer; + multicorrelator_cpu_16sc.free(); +} + + + +int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // Block input data and block output stream pointers + const lv_16sc_t* in = (lv_16sc_t*) input_items[0]; //PRN start block alignment + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + // process vars + double code_error_filt_secs_Ti = 0.0; + double CURRENT_INTEGRATION_TIME_S = 0.0; + double CORRECTED_INTEGRATION_TIME_S = 0.0; + + if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) + { + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter += samples_offset; // count for the processed samples + d_pull_in = false; + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs=d_fs_in; + *out[0] = current_synchro_data; + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc, in); + multicorrelator_cpu_16sc.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + + // ####### coherent intergration extension + // keep the last symbols + d_E_history.push_back(d_correlator_outs_16sc[0]); // save early output + d_P_history.push_back(d_correlator_outs_16sc[1]); // save prompt output + d_L_history.push_back(d_correlator_outs_16sc[2]); // save late output + + if (static_cast(d_P_history.size()) > d_extend_correlation_ms) + { + d_E_history.pop_front(); + d_P_history.pop_front(); + d_L_history.pop_front(); + } + + bool enable_dll_pll; + if (d_enable_extended_integration == true) + { + long int symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) + { + // compute coherent integration and enable tracking loop + // perform coherent integration using correlator output history + // std::cout<<"##### RESET COHERENT INTEGRATION ####"<PRN) + << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl + << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; + } + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_extend_correlation_ms) * GLONASS_L1_CA_CODE_PERIOD; + enable_dll_pll = true; + } + else + { + if(d_preamble_synchronized == true) + { + // continue extended coherent correlation + // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + int K_prn_samples = round(T_prn_samples); + double K_T_prn_error_samples = K_prn_samples - T_prn_samples; + + d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples; + d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples + d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; + d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast(d_correlation_length_samples), GLONASS_TWO_PI); + + // UPDATE ACCUMULATED CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI; + + // disable tracking loop and inform telemetry decoder + enable_dll_pll = false; + } + else + { + // perform basic (1ms) correlation + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + enable_dll_pll = true; + } + } + } + else + { + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + enable_dll_pll = true; + } + + if (enable_dll_pll == true) + { + // ################## PLL ########################################################## + // Update PLL discriminator [rads/Ti -> Secs/Ti] + d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / GLONASS_TWO_PI; //prompt output + + // Carrier discriminator filter + // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan + // Input [s/Ti] -> output [Hz] + d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S); + // PLL to DLL assistance [Secs/Ti] + d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / d_glonass_freq_ch; + // code Doppler frequency update + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); + + // ################## DLL ########################################################## + // DLL discriminator + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); // [chips/Ti] //early and late + // Code discriminator filter + d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] + d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; + code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_prn_samples = round(T_prn_samples); + double K_T_prn_error_samples = K_prn_samples - T_prn_samples; + + d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast(d_fs_in); + d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples + d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; + d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; + + //################### PLL COMMANDS ################################################# + //carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI; + // UPDATE ACCUMULATED CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(d_fs_in)); + //remnant carrier phase [rad] + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI); + + //################### DLL COMMANDS ################################################# + //code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ####################################### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = lv_cmake(static_cast(d_correlator_outs_16sc[1].real()), static_cast(d_correlator_outs_16sc[1].imag()) ); // prompt + d_cn0_estimation_counter++; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GLONASS_L1_CA_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); + // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + if (d_preamble_synchronized == true) + { + current_synchro_data.correlation_length_ms = d_extend_correlation_ms; + } + else + { + current_synchro_data.correlation_length_ms = 1; + } + } + else + { + current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + } + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs_16sc[n] = lv_cmake(0,0); + } + + current_synchro_data.System = {'R'}; + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + } + current_synchro_data.fs=d_fs_in; + *out[0] = current_synchro_data; + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs_16sc[1].real(); + prompt_Q = d_correlator_outs_16sc[1].imag(); + tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + //PLL commands + d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + + //DLL commands + d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "Exception writing trk dump file " << e->what(); + } + } + + consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_correlation_length_samples; //count for the processed samples + + return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_sc::set_channel(unsigned int channel) +{ + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl; + } + catch (const std::ifstream::failure* e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl; + } + } + } +} + + +void glonass_l1_ca_dll_pll_c_aid_tracking_sc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h new file mode 100644 index 000000000..dbc0a084a --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h @@ -0,0 +1,170 @@ +#ifndef GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_SC_H +#define GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_SC_H + +#include +#include +#include +#include +#include +#include +#include +#include "glonass_l1_signal_processing.h" +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_FLL_PLL_filter.h" +#include "cpu_multicorrelator_16sc.h" + +class glonass_l1_ca_dll_pll_c_aid_tracking_sc; + +typedef boost::shared_ptr + glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr; + +glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr +glonass_l1_ca_dll_pll_c_aid_make_tracking_sc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class glonass_l1_ca_dll_pll_c_aid_tracking_sc: public gr::block +{ +public: + ~glonass_l1_ca_dll_pll_c_aid_tracking_sc(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_tracking(); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + +private: + friend glonass_l1_ca_dll_pll_c_aid_tracking_sc_sptr + glonass_l1_ca_dll_pll_c_aid_make_tracking_sc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + glonass_l1_ca_dll_pll_c_aid_tracking_sc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + int extend_correlation_ms, + float early_late_space_chips); + + // tracking configuration vars + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + + long d_if_freq; + long d_fs_in; + long d_glonass_freq_ch; + + double d_early_late_spc_chips; + int d_n_correlator_taps; + + gr_complex* d_ca_code; + lv_16sc_t* d_ca_code_16sc; + float* d_local_code_shift_chips; + //gr_complex* d_correlator_outs; + lv_16sc_t* d_correlator_outs_16sc; + //cpu_multicorrelator multicorrelator_cpu; + cpu_multicorrelator_16sc multicorrelator_cpu_16sc; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_code_phase_chips; + double d_rem_carrier_phase_rad; + int d_rem_code_phase_integer_samples; + + // PLL and DLL filter library + Tracking_2nd_DLL_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 + float d_dll_bw_hz; + float d_pll_bw_hz; + float d_dll_bw_narrow_hz; + float d_pll_bw_narrow_hz; + double d_code_freq_chips; + double d_code_phase_step_chips; + double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; + double d_acc_carrier_phase_cycles; + double d_code_phase_samples; + double d_pll_to_dll_assist_secs_Ti; + double d_carr_phase_error_secs_Ti; + double d_code_error_chips_Ti; + double d_preamble_timestamp_s; + int d_extend_correlation_ms; + bool d_enable_extended_integration; + bool d_preamble_synchronized; + double d_code_error_filt_chips_s; + double d_code_error_filt_chips_Ti; + void msg_handler_preamble_index(pmt::pmt_t msg); + + // symbol history to detect bit transition + std::deque d_E_history; + std::deque d_P_history; + std::deque d_L_history; + + //Integration period in samples + int d_correlation_length_samples; + + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + gr_complex* d_Prompt_buffer; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + int d_carrier_lock_fail_counter; + + // control vars + bool d_enable_tracking; + bool d_pull_in; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GLONASS_L1_CA_DLL_PLL_C_AID_TRACKING_SC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc new file mode 100644 index 000000000..ad7b16cd6 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -0,0 +1,513 @@ +#include "glonass_l1_ca_dll_pll_tracking_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "glonass_l1_signal_processing.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "Glonass_L1_CA.h" +#include "control_message_factory.h" + + +/*! + * \todo Include in definition header file + */ +#define CN0_ESTIMATION_SAMPLES 20 +#define MINIMUM_VALID_CN0 25 +#define MAXIMUM_LOCK_FAIL_COUNTER 50 +#define CARRIER_LOCK_THRESHOLD 0.85 + + +using google::LogMessage; + +glonass_l1_ca_dll_pll_tracking_cc_sptr +glonass_l1_ca_dll_pll_make_tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) +{ + return glonass_l1_ca_dll_pll_tracking_cc_sptr(new Glonass_L1_Ca_Dll_Pll_Tracking_cc(if_freq, + fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); +} + + + +void Glonass_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } +} + + + +Glonass_L1_Ca_Dll_Pll_Tracking_cc::Glonass_L1_Ca_Dll_Pll_Tracking_cc( + long if_freq, + long fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) : + gr::block("Glonass_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_out(pmt::mp("events")); + + // initialize internal vars + d_dump = dump; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + + d_current_prn_length_samples = static_cast(d_vector_length); + + // Initialize tracking ========================================== + d_code_loop_filter.set_DLL_BW(dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + + //--- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the C/A code replica sampled 1x/chip + d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 3; // Early, Prompt, and Late + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + + multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps); + + //--- Perform initializations ------------------------------ + // define initial code frequency basis of NCO + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ; + // define residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // define residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + //d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; + + d_enable_tracking = false; + d_pull_in = false; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + + systemName["R"] = std::string("Glonass"); + + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_code_phase_samples = 0.0; + d_rem_code_phase_chips = 0.0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + + set_relative_rate(1.0 / static_cast(d_vector_length)); +} + + +void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() +{ + /* + * correct the code phase according to the delay between acq and trk + */ + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + d_glonass_freq_ch = GLONASS_L1_FREQ_HZ + (GLONASS_L1_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_current_prn_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + if (corrected_acq_phase_samples < 0) + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } + delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + + // generate local reference ALWAYS starting at chip 1 (1 sample per chip) + glonass_l1_ca_code_gen_complex(d_ca_code, 0); + + multicorrelator_cpu.set_local_code_and_taps(static_cast(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + + d_code_phase_samples = d_acq_code_phase_samples; + + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + + // DEBUG OUTPUT + std::cout << "Tracking of GLONASS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking + d_pull_in = true; + d_enable_tracking = true; + + LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; +} + + +Glonass_L1_Ca_Dll_Pll_Tracking_cc::~Glonass_L1_Ca_Dll_Pll_Tracking_cc() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } + try + { + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_ca_code); + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + + +int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // process vars + double carr_error_hz = 0.0; + double carr_error_filt_hz = 0.0; + double code_error_chips = 0.0; + double code_error_filt_chips = 0.0; + + // Block input data and block output stream pointers + const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment + Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; + + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) + { + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + d_pull_in = false; + // take into account the carrier cycles accumulated in the pull in signal alignment + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs = d_fs_in; + current_synchro_data.correlation_length_ms = 1; + *out[0] = current_synchro_data; + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } + + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); + + // ################## PLL ########################################################## + // PLL discriminator + // Update PLL discriminator [rads/Ti -> Secs/Ti] + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output + // Carrier discriminator filter + carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; + d_code_freq_chips = GLONASS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GLONASS_L1_CA_CODE_RATE_HZ) / d_glonass_freq_ch); + + // ################## DLL ########################################################## + // DLL discriminator + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + // Code discriminator filter + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds] + //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GLONASS_L1_CA_CODE_RATE_HZ; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + //double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GLONASS_TWO_PI); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt + d_cn0_estimation_counter++; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GLONASS_L1_CA_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = 1; + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.System = {'R'}; + current_synchro_data.correlation_length_ms = 1; + } + + //assign the GNURadio block output data + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + unsigned long int tmp_long; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + tmp_long = d_sample_counter + d_current_prn_length_samples; + d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + // PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); + + // DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } + + consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates + d_sample_counter += d_current_prn_length_samples; // count for the processed samples + return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false +} + + + +void Glonass_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) +{ + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void Glonass_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h new file mode 100644 index 000000000..0c40ef9fd --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h @@ -0,0 +1,135 @@ +#ifndef GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_CC_H +#define GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_CC_H + +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include "cpu_multicorrelator.h" + +class Glonass_L1_Ca_Dll_Pll_Tracking_cc; + +typedef boost::shared_ptr + glonass_l1_ca_dll_pll_tracking_cc_sptr; + +glonass_l1_ca_dll_pll_tracking_cc_sptr +glonass_l1_ca_dll_pll_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + + +/*! + * \brief This class implements a DLL + PLL tracking loop block + */ +class Glonass_L1_Ca_Dll_Pll_Tracking_cc: public gr::block +{ +public: + ~Glonass_L1_Ca_Dll_Pll_Tracking_cc(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_tracking(); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + +private: + friend glonass_l1_ca_dll_pll_tracking_cc_sptr + glonass_l1_ca_dll_pll_make_tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + Glonass_L1_Ca_Dll_Pll_Tracking_cc(long if_freq, + long fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips); + + // tracking configuration vars + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + + long d_if_freq; + long d_fs_in; + long d_glonass_freq_ch; + + double d_early_late_spc_chips; + + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_code_phase_chips; + double d_rem_carr_phase_rad; + + // PLL and DLL filter library + Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_2nd_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + // correlator + int d_n_correlator_taps; + gr_complex* d_ca_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator multicorrelator_cpu; + + + // tracking vars + double d_code_freq_chips; + double d_code_phase_step_chips; + double d_carrier_doppler_hz; + double d_carrier_phase_step_rad; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + + //PRN period in samples + int d_current_prn_length_samples; + + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + gr_complex* d_Prompt_buffer; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + int d_carrier_lock_fail_counter; + + // control vars + bool d_enable_tracking; + bool d_pull_in; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GLONASS_L1_CA_DLL_PLL_TRACKING_CC_H diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 0e85d4f2d..7cba2a649 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -78,6 +78,7 @@ #include "galileo_e1_pcps_cccwsr_ambiguous_acquisition.h" #include "galileo_e1_pcps_quicksync_ambiguous_acquisition.h" #include "galileo_e5a_noncoherent_iq_acquisition_caf.h" +#include "glonass_l1_ca_pcps_acquisition.h" #include "gps_l1_ca_dll_pll_tracking.h" #include "gps_l1_ca_dll_pll_c_aid_tracking.h" #include "gps_l1_ca_tcp_connector_tracking.h" @@ -85,6 +86,8 @@ #include "galileo_e1_tcp_connector_tracking.h" #include "galileo_e5a_dll_pll_tracking.h" #include "gps_l2_m_dll_pll_tracking.h" +#include "glonass_l1_ca_dll_pll_tracking.h" +#include "glonass_l1_ca_dll_pll_c_aid_tracking.h" #include "gps_l1_ca_telemetry_decoder.h" #include "gps_l2c_telemetry_decoder.h" #include "galileo_e1b_telemetry_decoder.h" @@ -228,7 +231,8 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared Galileo_channels += configuration->property("Channels_5X.count", 0); unsigned int GPS_channels = configuration->property("Channels_1C.count", 0); GPS_channels += configuration->property("Channels_2S.count", 0); - return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels, Galileo_channels + GPS_channels); + unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); + return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels + Glonass_channels, Galileo_channels + GPS_channels + Glonass_channels); } @@ -242,7 +246,8 @@ std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptrproperty("Channels_5X.count", 0); unsigned int GPS_channels = configuration->property("Channels_1C.count", 0); GPS_channels += configuration->property("Channels_2S.count", 0); - return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 0); + unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); + return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels + Glonass_channels, 0); } @@ -513,6 +518,74 @@ std::unique_ptr GNSSBlockFactory::GetChannel_5X( return channel_; } +//********* GLONASS L1 C/A CHANNEL ***************** +std::unique_ptr GNSSBlockFactory::GetChannel_1G( + std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + boost::shared_ptr queue) +{ + std::stringstream stream; + stream << channel; + std::string id = stream.str(); + LOG(INFO) << "Instantiating Channel " << channel << " with Acquisition Implementation: " + << acq << ", Tracking Implementation: " << trk << ", Telemetry Decoder implementation: " << tlm; + + std::string aux = configuration->property("Acquisition_1G" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix1; + if(aux.compare("W") != 0) + { + appendix1 = boost::lexical_cast(channel); + } + else + { + appendix1 = ""; + } + aux = configuration->property("Tracking_1G" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix2; + if(aux.compare("W") != 0) + { + appendix2 = boost::lexical_cast(channel); + } + else + { + appendix2 = ""; + } + aux = configuration->property("TelemetryDecoder_1G" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix3; + if(aux.compare("W") != 0) + { + appendix3 = boost::lexical_cast(channel); + } + else + { + appendix3 = ""; + } + // Automatically detect input data type + std::shared_ptr config; + config = std::make_shared(); + std::string default_item_type = "gr_complex"; + std::string acq_item_type = configuration->property("Acquisition_1G" + appendix1 + ".item_type", default_item_type); + std::string trk_item_type = configuration->property("Tracking_1G" + appendix2 + ".item_type", default_item_type); + if(acq_item_type.compare(trk_item_type)) + { + LOG(ERROR) << "Acquisition and Tracking blocks must have the same input data type!"; + } + config->set_property("Channel.item_type", acq_item_type); + + std::unique_ptr pass_through_ = GetBlock(config, "Channel", "Pass_Through", 1, 1, queue); + std::unique_ptr acq_ = GetAcqBlock(configuration, "Acquisition_1G" + appendix1, acq, 1, 0); + std::unique_ptr trk_ = GetTrkBlock(configuration, "Tracking_1G"+ appendix2, trk, 1, 1); + std::unique_ptr tlm_ = GetTlmBlock(configuration, "TelemetryDecoder_1G" + appendix3, tlm, 1, 1); + + std::unique_ptr channel_(new Channel(configuration.get(), channel, std::move(pass_through_), + std::move(acq_), + std::move(trk_), + std::move(tlm_), + "Channel", "1G", queue)); + + return channel_; +} + std::unique_ptr>> GNSSBlockFactory::GetChannels( std::shared_ptr configuration, boost::shared_ptr queue) @@ -528,11 +601,13 @@ std::unique_ptr>> GNSSBlockFacto unsigned int Channels_2S_count = configuration->property("Channels_2S.count", 0); unsigned int Channels_1B_count = configuration->property("Channels_1B.count", 0); unsigned int Channels_5X_count = configuration->property("Channels_5X.count", 0); + unsigned int Channels_1G_count = configuration->property("Channels_1G.count", 0); unsigned int total_channels = Channels_1C_count + Channels_2S_count + Channels_1B_count + - Channels_5X_count; + Channels_5X_count + + Channels_1G_count; std::unique_ptr>> channels(new std::vector>(total_channels)); //**************** GPS L1 C/A CHANNELS ********************** @@ -656,6 +731,37 @@ std::unique_ptr>> GNSSBlockFacto channel_absolute_id++; } + + //**************** GLONASS L1 C/A CHANNELS ********************** + LOG(INFO) << "Getting " << Channels_1G_count << " GLONASS L1 C/A channels"; + acquisition_implementation = configuration->property("Acquisition_1G.implementation", default_implementation); + tracking_implementation = configuration->property("Tracking_1G.implementation", default_implementation); + telemetry_decoder_implementation = configuration->property("TelemetryDecoder_1G.implementation", default_implementation); + + for (unsigned int i = 0; i < Channels_1G_count; i++) + { + //(i.e. Acquisition_1G0.implementation=xxxx) + std::string acquisition_implementation_specific = configuration->property( + "Acquisition_1G" + boost::lexical_cast(channel_absolute_id) + ".implementation", + acquisition_implementation); + //(i.e. Tracking_1G0.implementation=xxxx) + std::string tracking_implementation_specific = configuration->property( + "Tracking_1G" + boost::lexical_cast(channel_absolute_id) + ".implementation", + tracking_implementation); + std::string telemetry_decoder_implementation_specific = configuration->property( + "TelemetryDecoder_1G" + boost::lexical_cast(channel_absolute_id) + ".implementation", + telemetry_decoder_implementation); + + // Push back the channel to the vector of channels + channels->at(channel_absolute_id) = std::move(GetChannel_1G(configuration, + acquisition_implementation_specific, + tracking_implementation_specific, + telemetry_decoder_implementation_specific, + channel_absolute_id, + queue)); + channel_absolute_id++; + } + return channels; } @@ -984,6 +1090,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_PCPS_Acquisition") == 0) + { + std::unique_ptr block_(new GlonassL1CaPcpsAcquisition(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } @@ -1047,6 +1159,18 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_DLL_PLL_Tracking") == 0) + { + std::unique_ptr block_(new GlonassL1CaDllPllTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } + else if (implementation.compare("GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) + { + std::unique_ptr block_(new GlonassL1CaDllPllCAidTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } // TELEMETRY DECODERS ---------------------------------------------------------- else if (implementation.compare("GPS_L1_CA_Telemetry_Decoder") == 0) @@ -1217,6 +1341,12 @@ std::unique_ptr GNSSBlockFactory::GetAcqBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GLONASS_L1_CA_PCPS_Acquisition") == 0) + { + std::unique_ptr block_(new GlonassL1CaPcpsAcquisition(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. @@ -1293,6 +1423,18 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( block = std::move(block_); } #endif + else if (implementation.compare("GLONASS_L1_CA_DLL_PLL_Tracking") == 0) + { + std::unique_ptr block_(new GlonassL1CaDllPllTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } + else if (implementation.compare("GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) + { + std::unique_ptr block_(new GlonassL1CaDllPllCAidTracking(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. diff --git a/src/core/receiver/gnss_block_factory.h b/src/core/receiver/gnss_block_factory.h index 13c614ee8..6dd6ae11f 100644 --- a/src/core/receiver/gnss_block_factory.h +++ b/src/core/receiver/gnss_block_factory.h @@ -94,6 +94,10 @@ private: std::string acq, std::string trk, std::string tlm, int channel, boost::shared_ptr queue); + std::unique_ptr GetChannel_1G(std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + boost::shared_ptr queue); + std::unique_ptr GetAcqBlock( std::shared_ptr configuration, std::string role, diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index cf1788cc0..4529a1fba 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -579,7 +579,8 @@ void GNSSFlowgraph::set_signals_list() unsigned int total_channels = configuration_->property("Channels_1C.count", 0) + configuration_->property("Channels_2S.count", 0) + configuration_->property("Channels_1B.count", 0) + - configuration_->property("Channels_5X.count", 0); + configuration_->property("Channels_5X.count", 0) + + configuration_->property("Channels_1G.count", 0); /* * Loop to create the list of GNSS Signals @@ -596,6 +597,9 @@ void GNSSFlowgraph::set_signals_list() 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}; + std::set available_glonass_prn = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, + 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}; + std::string sv_list = configuration_->property("Galileo.prns", std::string("") ); if( sv_list.length() > 0 ) @@ -644,6 +648,22 @@ void GNSSFlowgraph::set_signals_list() } } + sv_list = configuration_->property("Glonass.prns", std::string("") ); + + if( sv_list.length() > 0 ) + { + // Reset the available prns: + std::set< unsigned int > tmp_set; + boost::tokenizer<> tok( sv_list ); + std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), + boost::lexical_cast ); + + if( tmp_set.size() > 0 ) + { + available_glonass_prn = tmp_set; + } + } + if (configuration_->property("Channels_1C.count", 0) > 0 ) { /* @@ -714,6 +734,20 @@ void GNSSFlowgraph::set_signals_list() *available_gnss_prn_iter), std::string("5X"))); } } + + if (configuration_->property("Channels_1G.count", 0) > 0 ) + { + /* + * Loop to create the list of GLONASS L1 C/A signals + */ + for (available_gnss_prn_iter = available_glonass_prn.begin(); + available_gnss_prn_iter != available_glonass_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Glonass"), + *available_gnss_prn_iter), std::string("1G"))); + } + } /* * Ordering the list of signals from configuration file */ @@ -727,6 +761,7 @@ void GNSSFlowgraph::set_signals_list() std::string gnss_system; if((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) ) gnss_system = "GPS"; if((gnss_signal.compare("1B") == 0) or (gnss_signal.compare("5X") == 0) ) gnss_system = "Galileo"; + if((gnss_signal.compare("1G") == 0)/*or (gnss_signal.compare("") == 0)*/) gnss_system = "Glonass"; unsigned int sat = configuration_->property("Channel" + boost::lexical_cast(i) + ".satellite", 0); LOG(INFO) << "Channel " << i << " system " << gnss_system << ", signal " << gnss_signal <<", sat "< +#include // std::map +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" + +// Physical constants +const double GLONASS_C_m_s = 299792458.0; //!< The speed of light, [m/s] +const double GLONASS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] +const double GLONASS_PI = 3.1415926535898; //!< Pi as (NOT) defined in ICD-GLONASS-2008 +const double GLONASS_TWO_PI = 6.283185307179586;//!< 2Pi as (NOT) defined in ICD-GLONASS-2008 +const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] +const double GLONASS_GM = 3.986004418e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] +// const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] + +// Geodesic constants and parameters +const double fMa = 0.35e9; //!< The Gravitational constant of atmosphere, [m^3/s^2] +const double SEMI_MAJOR_AXIS = 6378136; //!< The Semi-major axis, [m] +const double FLATTENING = 1/298.25784; //!< The Orbital Flattening +const double EQUATORIAL_GRAVITY = 978032.84; //!< The Equatorial acceleration of gravity, [mGal] +const double GRAVITY_CORRECTION = 0.87; //!< The Correction to acceleration of gravity at sea-level due to Atmosphere, [mGal] +const double SECOND_HARMONIC = 1082625.75e-9; //!< Second zonal harmonic of the geopotential (J_2^0) +const double FOURTH_HARMONIC = -2370.89e-9; //!< Fourth zonal harmonic of the geopotential (J_4^0) +const double SIXTH_HARMONIC = 6.08e-9; //!< Sixth zonal harmonic of the geopotential (J_6^0) +const double EIGHTH_HARMONIC = 1.40e-11; //!< Eighth zonal harmonic of the geopotential (J_8^0) +const double NORMAL_POTENCIAL = 62636861.4; //!< The Normal potential at surface of common terrestrial ellipsoid (U_0), [m^2/s^2] + + +// carrier and code frequencies +const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] +const double GLONASS_L1_CA_CODE_RATE_HZ = 0.511e6; //!< GLONASS L1 C/A code rate [chips/s] +const double GLONASS_L1_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips] +const double GLONASS_L1_CA_CODE_PERIOD = 0.001; //!< GLONASS L1 C/A code period [seconds] +const double GLONASS_L1_CA_CHIP_PERIOD = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds] + +// GLONASS SV's orbital slots PRN = (orbital_slot - 1) +const std::map GLONASS_PRN = + {{ 0, 8,}, //For test + { 1, 1,}, //Plane 1 + { 2,-4,}, //Plane 1 + { 3, 5,}, //Plane 1 + { 4, 6,}, //Plane 1 + { 5, 1,}, //Plane 1 + { 6,-4,}, //Plane 1 + { 7, 5,}, //Plane 1 + { 8, 6,}, //Plane 1 + { 9,-2,}, //Plane 2 + {10,-7,}, //Plane 2 + {11, 0,}, //Plane 2 + {12,-1,}, //Plane 2 + {13,-2,}, //Plane 2 + {14,-7,}, //Plane 2 + {15, 0,}, //Plane 2 + {16,-1,}, //Plane 2 + {17, 4,}, //Plane 3 + {18,-3,}, //Plane 3 + {19, 3,}, //Plane 3 + {20, 2,}, //Plane 3 + {21, 4,}, //Plane 3 + {22,-3,}, //Plane 3 + {23, 3,}, //Plane 3 + {24, 2}}; //Plane 3 + + +const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] + +#endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 2e2e18f00..b192224fb 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -61,9 +61,9 @@ Gnss_Satellite::~Gnss_Satellite() void Gnss_Satellite::reset() { - system_set = {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}; + system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}; satelliteSystem["GPS"] = "G"; - satelliteSystem["GLONASS"] = "R"; + satelliteSystem["Glonass"] = "R"; satelliteSystem["SBAS"] = "S"; satelliteSystem["Galileo"] = "E"; satelliteSystem["Beidou"] = "C"; @@ -120,8 +120,8 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite &rhs) { void Gnss_Satellite::set_system(const std::string& system_) { - // Set the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"} - std::set::const_iterator it = system_set.find(system_); + // Set the satellite system {"GPS", "Glonass", "SBAS", "Galileo", "Compass"} + std::set::iterator it = system_set.find(system_); if(it != system_set.cend()) { @@ -129,7 +129,7 @@ void Gnss_Satellite::set_system(const std::string& system_) } else { - DLOG(INFO) << "System " << system_ << " is not defined {GPS, GLONASS, SBAS, Galileo, Beidou}. Initialization?"; + DLOG(INFO) << "System " << system_ << " is not defined {GPS, Glonass, SBAS, Galileo, Beidou}. Initialization?"; system = std::string(""); } } @@ -220,7 +220,7 @@ unsigned int Gnss_Satellite::get_PRN() const std::string Gnss_Satellite::get_system() const { - // Get the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"} + // Get the satellite system {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"} std::string system_; system_ = system; return system_; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 4922e5399..91b6f7162 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -51,7 +51,7 @@ public: Gnss_Signal(const std::string& signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_); ~Gnss_Signal(); - std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a} + std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1G" for GLONASS L1 C/A, "1B" for Galileo E1B, "5X" for Galileo E5a} Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal friend bool operator== (const Gnss_Signal &, const Gnss_Signal &); //!< operator== for comparison friend std::ostream& operator<<(std::ostream &, const Gnss_Signal &); //!< operator<< for pretty printing diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 4403d3616..1efed9015 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -267,11 +267,15 @@ if(ENABLE_INSTALL_TESTS) install(FILES ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat DESTINATION share/gnss-sdr/signal_samples) install(FILES ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/Galileo_E1_ID_1_Fs_4Msps_8ms.dat DESTINATION share/gnss-sdr/signal_samples) install(FILES ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GPS_L1_CA_ID_1_Fs_4Msps_2ms.dat DESTINATION share/gnss-sdr/signal_samples) + install(FILES ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_4ms.dat DESTINATION share/gnss-sdr/signal_samples) + install(FILES ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin DESTINATION share/gnss-sdr/signal_samples) add_definitions(-DTEST_PATH="${CMAKE_INSTALL_PREFIX}/share/gnss-sdr/") else(ENABLE_INSTALL_TESTS) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/Galileo_E1_ID_1_Fs_4Msps_8ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GPS_L1_CA_ID_1_Fs_4Msps_2ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) + file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_4ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) + file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) add_definitions(-DTEST_PATH="${CMAKE_SOURCE_DIR}/thirdparty/") endif(ENABLE_INSTALL_TESTS) diff --git a/src/tests/signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_4ms.dat b/src/tests/signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_4ms.dat new file mode 100644 index 000000000..8d7b27c25 Binary files /dev/null and b/src/tests/signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_4ms.dat differ diff --git a/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin b/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin new file mode 100644 index 000000000..bba434e4c Binary files /dev/null and b/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin differ diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index c149edc2c..b86bde939 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -101,6 +101,8 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc" +#include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc" +#include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc" //#include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_multithread_acquisition_gsoc2013_test.cc" #if OPENCL_BLOCKS_TEST #include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc" @@ -127,6 +129,8 @@ DECLARE_string(log_dir); #if EXTRA_TESTS #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc" #if MODERN_ARMADILLO #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc new file mode 100644 index 000000000..a346cf12f --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc @@ -0,0 +1,618 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_block_interface.h" +#include "in_memory_configuration.h" +#include "configuration_interface.h" +#include "gnss_synchro.h" +#include "glonass_l1_ca_pcps_acquisition.h" +#include "signal_generator.h" +#include "signal_generator_c.h" +#include "fir_filter.h" +#include "gen_signal_source.h" +#include "gnss_sdr_valve.h" +#include "boost/shared_ptr.hpp" +#include "pass_through.h" + + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx; + +typedef boost::shared_ptr GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_sptr; + +GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_sptr GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(concurrent_queue& queue); + + +class GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx : public gr::block +{ +private: + friend GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_sptr GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(concurrent_queue& queue); + void msg_handler_events(pmt::pmt_t msg); + GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx(concurrent_queue& queue); + concurrent_queue& channel_internal_queue; +public: + int rx_message; + ~GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx(); //!< Default destructor +}; + + +GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_sptr GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(concurrent_queue& queue) +{ + return GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_sptr(new GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx(queue)); +} + + +void GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + channel_internal_queue.push(rx_message); + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + + +GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx(concurrent_queue& queue) : + gr::block("GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)), channel_internal_queue(queue) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + +GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx::~GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx() +{} + + +// ########################################################### + +class GlonassL1CaPcpsAcquisitionGSoC2017Test: public ::testing::Test +{ +protected: + GlonassL1CaPcpsAcquisitionGSoC2017Test() + { + item_size = sizeof(gr_complex); + stop = false; + message = 0; + gnss_synchro = Gnss_Synchro(); + acquisition = 0; + init(); + } + + ~GlonassL1CaPcpsAcquisitionGSoC2017Test() + { + } + + void init(); + void config_1(); + void config_2(); + void start_queue(); + void wait_message(); + void process_message(); + void stop_queue(); + + concurrent_queue channel_internal_queue; + + gr::msg_queue::sptr queue; + gr::top_block_sptr top_block; + GlonassL1CaPcpsAcquisition *acquisition; + std::shared_ptr config; + Gnss_Synchro gnss_synchro; + size_t item_size; + bool stop; + int message; + boost::thread ch_thread; + + unsigned int integration_time_ms = 0; + unsigned int fs_in = 0; + + double expected_delay_chips = 0.0; + double expected_doppler_hz = 0.0; + float max_doppler_error_hz = 0.0; + float max_delay_error_chips = 0.0; + + unsigned int num_of_realizations = 0; + unsigned int realization_counter; + unsigned int detection_counter; + unsigned int correct_estimation_counter; + unsigned int acquired_samples; + unsigned int mean_acq_time_us; + + double mse_doppler; + double mse_delay; + + double Pd; + double Pfa_p; + double Pfa_a; +}; + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::init() +{ + message = 0; + realization_counter = 0; + detection_counter = 0; + correct_estimation_counter = 0; + acquired_samples = 0; + mse_doppler = 0; + mse_delay = 0; + mean_acq_time_us = 0; + Pd = 0; + Pfa_p = 0; + Pfa_a = 0; +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_1() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'R'; + std::string signal = "1G"; + signal.copy(gnss_synchro.Signal,2,0); + + integration_time_ms = 1; + fs_in = 31.75e6; + + expected_delay_chips = 255; + expected_doppler_hz = -1500; + max_doppler_error_hz = 2/(3*integration_time_ms*1e-3); + max_delay_error_chips = 0.50; + + num_of_realizations = 1; + + config = std::make_shared(); + + config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in)); + + config->set_property("SignalSource.fs_hz", std::to_string(fs_in)); + + config->set_property("SignalSource.item_type", "gr_complex"); + + config->set_property("SignalSource.num_satellites", "1"); + + config->set_property("SignalSource.system_0", "R"); + config->set_property("SignalSource.PRN_0", "10"); + config->set_property("SignalSource.CN0_dB_0", "44"); + config->set_property("SignalSource.doppler_Hz_0", std::to_string(expected_doppler_hz)); + config->set_property("SignalSource.delay_chips_0", std::to_string(expected_delay_chips)); + + config->set_property("SignalSource.noise_flag", "false"); + config->set_property("SignalSource.data_flag", "false"); + config->set_property("SignalSource.BW_BB", "0.97"); + + config->set_property("InputFilter.implementation", "Fir_Filter"); + config->set_property("InputFilter.input_item_type", "gr_complex"); + config->set_property("InputFilter.output_item_type", "gr_complex"); + config->set_property("InputFilter.taps_item_type", "float"); + config->set_property("InputFilter.number_of_taps", "11"); + config->set_property("InputFilter.number_of_bands", "2"); + config->set_property("InputFilter.band1_begin", "0.0"); + config->set_property("InputFilter.band1_end", "0.97"); + config->set_property("InputFilter.band2_begin", "0.98"); + config->set_property("InputFilter.band2_end", "1.0"); + config->set_property("InputFilter.ampl1_begin", "1.0"); + config->set_property("InputFilter.ampl1_end", "1.0"); + config->set_property("InputFilter.ampl2_begin", "0.0"); + config->set_property("InputFilter.ampl2_end", "0.0"); + config->set_property("InputFilter.band1_error", "1.0"); + config->set_property("InputFilter.band2_error", "1.0"); + config->set_property("InputFilter.filter_type", "bandpass"); + config->set_property("InputFilter.grid_density", "16"); + + config->set_property("Acquisition.item_type", "gr_complex"); + config->set_property("Acquisition.if", "4000000"); + config->set_property("Acquisition.coherent_integration_time_ms", + std::to_string(integration_time_ms)); + config->set_property("Acquisition.max_dwells", "1"); + config->set_property("Acquisition.implementation", "GLONASS_L1_CA_PCPS_Acquisition"); + config->set_property("Acquisition.threshold", "0.8"); + config->set_property("Acquisition.doppler_max", "10000"); + config->set_property("Acquisition.doppler_step", "250"); + config->set_property("Acquisition.bit_transition_flag", "false"); + config->set_property("Acquisition.dump", "false"); +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::config_2() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'R'; + std::string signal = "1G"; + signal.copy(gnss_synchro.Signal,2,0); + + integration_time_ms = 1; + fs_in = 31.75e6; + + expected_delay_chips = 374; + expected_doppler_hz = -2000; + max_doppler_error_hz = 2/(3*integration_time_ms*1e-3); + max_delay_error_chips = 0.50; + + num_of_realizations = 100; + + config = std::make_shared(); + + config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in)); + + config->set_property("SignalSource.fs_hz", std::to_string(fs_in)); + + config->set_property("SignalSource.item_type", "gr_complex"); + + config->set_property("SignalSource.num_satellites", "4"); + + config->set_property("SignalSource.system_0", "R"); + config->set_property("SignalSource.PRN_0", "10"); + config->set_property("SignalSource.CN0_dB_0", "44"); + config->set_property("SignalSource.doppler_Hz_0", std::to_string(expected_doppler_hz)); + config->set_property("SignalSource.delay_chips_0", std::to_string(expected_delay_chips)); + + config->set_property("SignalSource.system_1", "R"); + config->set_property("SignalSource.PRN_1", "15"); + config->set_property("SignalSource.CN0_dB_1", "44"); + config->set_property("SignalSource.doppler_Hz_1", "1000"); + config->set_property("SignalSource.delay_chips_1", "100"); + + config->set_property("SignalSource.system_2", "R"); + config->set_property("SignalSource.PRN_2", "21"); + config->set_property("SignalSource.CN0_dB_2", "44"); + config->set_property("SignalSource.doppler_Hz_2", "2000"); + config->set_property("SignalSource.delay_chips_2", "200"); + + config->set_property("SignalSource.system_3", "R"); + config->set_property("SignalSource.PRN_3", "22"); + config->set_property("SignalSource.CN0_dB_3", "44"); + config->set_property("SignalSource.doppler_Hz_3", "3000"); + config->set_property("SignalSource.delay_chips_3", "300"); + + config->set_property("SignalSource.noise_flag", "true"); + config->set_property("SignalSource.data_flag", "true"); + config->set_property("SignalSource.BW_BB", "0.97"); + + config->set_property("InputFilter.implementation", "Fir_Filter"); + config->set_property("InputFilter.input_item_type", "gr_complex"); + config->set_property("InputFilter.output_item_type", "gr_complex"); + config->set_property("InputFilter.taps_item_type", "float"); + config->set_property("InputFilter.number_of_taps", "11"); + config->set_property("InputFilter.number_of_bands", "2"); + config->set_property("InputFilter.band1_begin", "0.0"); + config->set_property("InputFilter.band1_end", "0.97"); + config->set_property("InputFilter.band2_begin", "0.98"); + config->set_property("InputFilter.band2_end", "1.0"); + config->set_property("InputFilter.ampl1_begin", "1.0"); + config->set_property("InputFilter.ampl1_end", "1.0"); + config->set_property("InputFilter.ampl2_begin", "0.0"); + config->set_property("InputFilter.ampl2_end", "0.0"); + config->set_property("InputFilter.band1_error", "1.0"); + config->set_property("InputFilter.band2_error", "1.0"); + config->set_property("InputFilter.filter_type", "bandpass"); + config->set_property("InputFilter.grid_density", "16"); + + config->set_property("Acquisition.item_type", "gr_complex"); + config->set_property("Acquisition.if", "4000000"); + config->set_property("Acquisition.coherent_integration_time_ms", + std::to_string(integration_time_ms)); + config->set_property("Acquisition.max_dwells", "1"); + config->set_property("Acquisition.implementation", "GLONASS_L1_CA_PCPS_Acquisition"); + config->set_property("Acquisition.pfa", "0.1"); + config->set_property("Acquisition.doppler_max", "10000"); + config->set_property("Acquisition.doppler_step", "250"); + config->set_property("Acquisition.bit_transition_flag", "false"); + config->set_property("Acquisition.dump", "false"); +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::start_queue() +{ + stop = false; + ch_thread = boost::thread(&GlonassL1CaPcpsAcquisitionGSoC2017Test::wait_message, this); +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::wait_message() +{ + struct timeval tv; + long long int begin = 0; + long long int end = 0; + + while (!stop) + { + acquisition->reset(); + + gettimeofday(&tv, NULL); + begin = tv.tv_sec *1e6 + tv.tv_usec; + + channel_internal_queue.wait_and_pop(message); + + gettimeofday(&tv, NULL); + end = tv.tv_sec *1e6 + tv.tv_usec; + + mean_acq_time_us += (end-begin); + + process_message(); + } +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::process_message() +{ + if (message == 1) + { + detection_counter++; + + // The term -5 is here to correct the additional delay introduced by the FIR filter + // The value 511.0 must be a variable, chips/length + double delay_error_chips = std::abs((double)expected_delay_chips - (double)(gnss_synchro.Acq_delay_samples-5)*511.0/((double)fs_in*1e-3)); + double doppler_error_hz = std::abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz); + + mse_delay += std::pow(delay_error_chips, 2); + mse_doppler += std::pow(doppler_error_hz, 2); + + if ((delay_error_chips < max_delay_error_chips) && (doppler_error_hz < max_doppler_error_hz)) + { + correct_estimation_counter++; + } + } + + realization_counter++; + + std::cout << "Progress: " << round((float)realization_counter/num_of_realizations*100) << "% \r" << std::flush; + + if (realization_counter == num_of_realizations) + { + mse_delay /= num_of_realizations; + mse_doppler /= num_of_realizations; + + Pd = (double)correct_estimation_counter / (double)num_of_realizations; + Pfa_a = (double)detection_counter / (double)num_of_realizations; + Pfa_p = (double)(detection_counter - correct_estimation_counter) / (double)num_of_realizations; + + mean_acq_time_us /= num_of_realizations; + + stop_queue(); + top_block->stop(); + } +} + +void GlonassL1CaPcpsAcquisitionGSoC2017Test::stop_queue() +{ + stop = true; +} + +TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, Instantiate) +{ + config_1(); + acquisition = new GlonassL1CaPcpsAcquisition(config.get(), "Acquisition", 1, 1); + delete acquisition; +} + +TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ConnectAndRun) +{ + int nsamples = floor(fs_in*integration_time_ms*1e-3); + struct timeval tv; + long long int begin = 0; + long long int end = 0; + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Acquisition test"); + + config_1(); + acquisition = new GlonassL1CaPcpsAcquisition(config.get(), "Acquisition", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(channel_internal_queue); + + ASSERT_NO_THROW( { + acquisition->connect(top_block); + boost::shared_ptr 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); + top_block->connect(source, 0, valve, 0); + top_block->connect(valve, 0, acquisition->get_left_block(), 0); + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of acquisition test."<< std::endl; + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec *1e6 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec *1e6 + tv.tv_usec; + }) << "Failure running the top_block."<< std::endl; + + std::cout << "Processed " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; + + delete acquisition; +} + +TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResults) +{ + config_1(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Acquisition test"); + + acquisition = new GlonassL1CaPcpsAcquisition(config.get(), "Acquisition", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(channel_internal_queue); + + ASSERT_NO_THROW( { + acquisition->set_channel(1); + }) << "Failure setting channel."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_max(10000); + }) << "Failure setting doppler_max."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_step(500); + }) << "Failure setting doppler_step."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_threshold(0.5); + }) << "Failure setting threshold."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->connect(top_block); + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting acquisition to the top_block."<< std::endl; + + acquisition->init(); + + ASSERT_NO_THROW( { + boost::shared_ptr signal_source; + SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue); + FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1); + signal_source.reset(new GenSignalSource(signal_generator, filter, "SignalSource", queue)); + signal_source->connect(top_block); + top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0); + }) << "Failure connecting the blocks of acquisition test." << std::endl; + + // i = 0 --> satellite in acquisition is visible + // i = 1 --> satellite in acquisition is not visible + for (unsigned int i = 0; i < 2; i++) + { + init(); + + if (i == 0) + { + gnss_synchro.PRN = 10; // This satellite is visible + } + else if (i == 1) + { + gnss_synchro.PRN = 20; // This satellite is not visible + } + + acquisition->set_local_code(); + acquisition->set_state(1); // Ensure that acquisition starts at the first sample + start_queue(); + + EXPECT_NO_THROW( { + top_block->run(); // Start threads and wait + }) << "Failure running the top_block."<< std::endl; + + if (i == 0) + { + EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; + if (message == 1) + { + EXPECT_EQ((unsigned int) 1, correct_estimation_counter) << "Acquisition failure. Incorrect parameters estimation."; + } + + } + else if (i == 1) + { + EXPECT_EQ(2, message) << "Acquisition failure. Expected message: 2=ACQ FAIL."; + } +#ifdef OLD_BOOST + ASSERT_NO_THROW( { + ch_thread.timed_join(boost::posix_time::seconds(1)); + }) << "Failure while waiting the queue to stop" << std::endl; +#endif +#ifndef OLD_BOOST + ASSERT_NO_THROW( { + ch_thread.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50)); + }) << "Failure while waiting the queue to stop" << std::endl; +#endif + } + + delete acquisition; +} + +TEST_F(GlonassL1CaPcpsAcquisitionGSoC2017Test, ValidationOfResultsProbabilities) +{ + config_2(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Acquisition test"); + acquisition = new GlonassL1CaPcpsAcquisition(config.get(), "Acquisition", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaPcpsAcquisitionGSoC2017Test_msg_rx_make(channel_internal_queue); + + ASSERT_NO_THROW( { + acquisition->set_channel(1); + }) << "Failure setting channel."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000)); + }) << "Failure setting doppler_max."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 500)); + }) << "Failure setting doppler_step."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->set_threshold(config->property("Acquisition.threshold", 0.0)); + }) << "Failure setting threshold."<< std::endl; + + ASSERT_NO_THROW( { + acquisition->connect(top_block); + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting acquisition to the top_block."<< std::endl; + + acquisition->init(); + + ASSERT_NO_THROW( { + boost::shared_ptr signal_source; + SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue); + FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1); + signal_source.reset(new GenSignalSource(signal_generator, filter, "SignalSource", queue)); + signal_source->connect(top_block); + top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0); + }) << "Failure connecting the blocks of acquisition test." << std::endl; + + std::cout << "Probability of false alarm (target) = " << 0.1 << std::endl; + + // i = 0 --> satellite in acquisition is visible (prob of detection and prob of detection with wrong estimation) + // i = 1 --> satellite in acquisition is not visible (prob of false detection) + for (unsigned int i = 0; i < 2; i++) + { + init(); + + if (i == 0) + { + gnss_synchro.PRN = 10; // This satellite is visible + } + else if (i == 1) + { + gnss_synchro.PRN = 1; // This satellite is not visible + } + + acquisition->set_local_code(); + + start_queue(); + + EXPECT_NO_THROW( { + top_block->run(); // Start threads and wait + }) << "Failure running the top_block."<< std::endl; + + if (i == 0) + { + std::cout << "Estimated probability of detection = " << Pd << std::endl; + std::cout << "Estimated probability of false alarm (satellite present) = " << Pfa_p << std::endl; + std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; } + else if (i == 1) + { + std::cout << "Estimated probability of false alarm (satellite absent) = " << Pfa_a << std::endl; + std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl; + } +#ifdef OLD_BOOST + ASSERT_NO_THROW( { + ch_thread.timed_join(boost::posix_time::seconds(1)); + }) << "Failure while waiting the queue to stop" << std::endl; +#endif +#ifndef OLD_BOOST + ASSERT_NO_THROW( { + ch_thread.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(50)); + }) << "Failure while waiting the queue to stop" << std::endl; +#endif + } + + delete acquisition; +} + diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc new file mode 100644 index 000000000..1a29bda80 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc @@ -0,0 +1,235 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_block_factory.h" +#include "gnss_block_interface.h" +#include "in_memory_configuration.h" +#include "gnss_sdr_valve.h" +#include "gnss_synchro.h" +#include "glonass_l1_ca_pcps_acquisition.h" + + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class GlonassL1CaPcpsAcquisitionTest_msg_rx; + +typedef boost::shared_ptr GlonassL1CaPcpsAcquisitionTest_msg_rx_sptr; + +GlonassL1CaPcpsAcquisitionTest_msg_rx_sptr GlonassL1CaPcpsAcquisitionTest_msg_rx_make(); + +class GlonassL1CaPcpsAcquisitionTest_msg_rx : public gr::block +{ +private: + friend GlonassL1CaPcpsAcquisitionTest_msg_rx_sptr GlonassL1CaPcpsAcquisitionTest_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + GlonassL1CaPcpsAcquisitionTest_msg_rx(); +public: + int rx_message; + ~GlonassL1CaPcpsAcquisitionTest_msg_rx(); //!< Default destructor +}; + + +GlonassL1CaPcpsAcquisitionTest_msg_rx_sptr GlonassL1CaPcpsAcquisitionTest_msg_rx_make() +{ + return GlonassL1CaPcpsAcquisitionTest_msg_rx_sptr(new GlonassL1CaPcpsAcquisitionTest_msg_rx()); +} + + +void GlonassL1CaPcpsAcquisitionTest_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch(boost::bad_any_cast& e) + { + std::cout << "msg_handler_telemetry Bad any cast!" << std::endl; + rx_message = 0; + } +} + + +GlonassL1CaPcpsAcquisitionTest_msg_rx::GlonassL1CaPcpsAcquisitionTest_msg_rx() : + gr::block("GlonassL1CaPcpsAcquisitionTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&GlonassL1CaPcpsAcquisitionTest_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + + +GlonassL1CaPcpsAcquisitionTest_msg_rx::~GlonassL1CaPcpsAcquisitionTest_msg_rx() +{} + + +// ########################################################### + +class GlonassL1CaPcpsAcquisitionTest: public ::testing::Test +{ +protected: + GlonassL1CaPcpsAcquisitionTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro = Gnss_Synchro(); + } + + ~GlonassL1CaPcpsAcquisitionTest() + {} + + void init(); + + gr::top_block_sptr top_block; + std::shared_ptr factory; + std::shared_ptr config; + Gnss_Synchro gnss_synchro; + size_t item_size; +}; + + +void GlonassL1CaPcpsAcquisitionTest::init() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'R'; + std::string signal = "1G"; + signal.copy(gnss_synchro.Signal, 2, 0); + gnss_synchro.PRN = 1; + config->set_property("GNSS-SDR.internal_fs_hz", "62314000"); + config->set_property("Acquisition.item_type", "gr_complex"); + config->set_property("Acquisition.if", "9540000"); + config->set_property("Acquisition.coherent_integration_time_ms", "1"); + config->set_property("Acquisition.dump", "true"); + config->set_property("Acquisition.dump_filename", "./acquisition.dat"); + config->set_property("Acquisition.implementation", "Glonass_L1_CA_PCPS_Acquisition"); + config->set_property("Acquisition.threshold", "0.001"); + config->set_property("Acquisition.doppler_max", "5000"); + config->set_property("Acquisition.doppler_step", "500"); + config->set_property("Acquisition.repeat_satellite", "false"); + config->set_property("Acquisition.pfa", "0.0"); +} + + + +TEST_F(GlonassL1CaPcpsAcquisitionTest, Instantiate) +{ + init(); + boost::shared_ptr acquisition = boost::make_shared(config.get(), "Acquisition", 1, 1); +} + +TEST_F(GlonassL1CaPcpsAcquisitionTest, ConnectAndRun) +{ + int fs_in = 62314000; + int nsamples = 62314; + struct timeval tv; + long long int begin = 0; + long long int end = 0; + gr::msg_queue::sptr queue = gr::msg_queue::make(0); + + top_block = gr::make_top_block("Acquisition test"); + init(); + boost::shared_ptr acquisition = boost::make_shared(config.get(), "Acquisition", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaPcpsAcquisitionTest_msg_rx_make(); + + ASSERT_NO_THROW( { + acquisition->connect(top_block); + boost::shared_ptr 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); + top_block->connect(source, 0, valve, 0); + top_block->connect(valve, 0, acquisition->get_left_block(), 0); + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + + }) << "Failure connecting the blocks of acquisition test." << std::endl; + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec * 1000000 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec * 1000000 + tv.tv_usec; + }) << "Failure running the top_block." << std::endl; + + std::cout << "Processed " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; +} + +TEST_F(GlonassL1CaPcpsAcquisitionTest, ValidationOfResults) +{ + struct timeval tv; + long long int begin = 0; + long long int end = 0; + top_block = gr::make_top_block("Acquisition test"); + + double expected_delay_samples = 31874; + double expected_doppler_hz = -9500; + init(); + std::shared_ptr acquisition = std::make_shared(config.get(), "Acquisition", 1, 1); + + boost::shared_ptr msg_rx = GlonassL1CaPcpsAcquisitionTest_msg_rx_make(); + + ASSERT_NO_THROW( { + acquisition->set_channel(1); + }) << "Failure setting channel." << std::endl; + + ASSERT_NO_THROW( { + acquisition->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro." << std::endl; + + ASSERT_NO_THROW( { + acquisition->set_threshold(0.001); + }) << "Failure setting threshold." << std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_max(10000); + }) << "Failure setting doppler_max." << std::endl; + + ASSERT_NO_THROW( { + acquisition->set_doppler_step(250); + }) << "Failure setting doppler_step." << std::endl; + + ASSERT_NO_THROW( { + acquisition->connect(top_block); + }) << "Failure connecting acquisition to the top_block." << std::endl; + + ASSERT_NO_THROW( { + std::string path = std::string(TEST_PATH); + std::string file = path + "signal_samples/Glonass_L1_CA_SIM_Fs_62Msps_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); + top_block->connect(file_source, 0, acquisition->get_left_block(), 0); + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of acquisition test." << std::endl; + + + acquisition->set_state(1); // Ensure that acquisition starts at the first sample + acquisition->init(); + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec * 1000000 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec * 1000000 + tv.tv_usec; + }) << "Failure running the top_block." << std::endl; + + + unsigned long int nsamples = gnss_synchro.Acq_samplestamp_samples; + std::cout << "Acquired " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; + + ASSERT_EQ(1, msg_rx->rx_message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; + + double delay_error_samples = std::abs(expected_delay_samples - gnss_synchro.Acq_delay_samples); + float delay_error_chips = (float)(delay_error_samples * 511 / 62316); + double doppler_error_hz = std::abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz); + + EXPECT_LE(doppler_error_hz, 666) << "Doppler error exceeds the expected value: 666 Hz = 2/(3*integration period)"; + EXPECT_LT(delay_error_chips, 0.5) << "Delay error exceeds the expected value: 0.5 chips"; +} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc new file mode 100644 index 000000000..9a65a43a6 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc @@ -0,0 +1,209 @@ +/*! + * \file glonass_l1_ca_dll_pll_c_aid_tracking_test.cc + * \brief This class implements a tracking test for GLONASS_L1_CA_DLL_PLL_Tracking + * implementation based on some input parameters. + * \author Gabriel Araujo, 2017. gabriel.araujo.5000(at)gmail.com + * \author Luis Esteve, 2017. luis(at)epsilon-formacion.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_block_factory.h" +#include "gnss_block_interface.h" +#include "tracking_interface.h" +#include "in_memory_configuration.h" +#include "gnss_sdr_valve.h" +#include "gnss_synchro.h" +#include "glonass_l1_ca_dll_pll_c_aid_tracking.h" + + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class GlonassL1CaDllPllCAidTrackingTest_msg_rx; + +typedef boost::shared_ptr GlonassL1CaDllPllCAidTrackingTest_msg_rx_sptr; + +GlonassL1CaDllPllCAidTrackingTest_msg_rx_sptr GlonassL1CaDllPllCAidTrackingTest_msg_rx_make(); + +class GlonassL1CaDllPllCAidTrackingTest_msg_rx : public gr::block +{ +private: + friend GlonassL1CaDllPllCAidTrackingTest_msg_rx_sptr GlonassL1CaDllPllCAidTrackingTest_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + GlonassL1CaDllPllCAidTrackingTest_msg_rx(); + +public: + int rx_message; + ~GlonassL1CaDllPllCAidTrackingTest_msg_rx(); //!< Default destructor + +}; + +GlonassL1CaDllPllCAidTrackingTest_msg_rx_sptr GlonassL1CaDllPllCAidTrackingTest_msg_rx_make() +{ + return GlonassL1CaDllPllCAidTrackingTest_msg_rx_sptr(new GlonassL1CaDllPllCAidTrackingTest_msg_rx()); +} + +void GlonassL1CaDllPllCAidTrackingTest_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + +GlonassL1CaDllPllCAidTrackingTest_msg_rx::GlonassL1CaDllPllCAidTrackingTest_msg_rx() : + gr::block("GlonassL1CaDllPllCAidTrackingTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&GlonassL1CaDllPllCAidTrackingTest_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + +GlonassL1CaDllPllCAidTrackingTest_msg_rx::~GlonassL1CaDllPllCAidTrackingTest_msg_rx() +{} + + +// ########################################################### + + +class GlonassL1CaDllPllCAidTrackingTest: public ::testing::Test +{ +protected: + GlonassL1CaDllPllCAidTrackingTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro = Gnss_Synchro(); + } + + ~GlonassL1CaDllPllCAidTrackingTest() + {} + + 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; +}; + + +void GlonassL1CaDllPllCAidTrackingTest::init() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'R'; + std::string signal = "1G"; + signal.copy(gnss_synchro.Signal, 2, 0); + gnss_synchro.PRN = 11; + + config->set_property("GNSS-SDR.internal_fs_hz", "6625000"); + config->set_property("Tracking_1G.item_type", "gr_complex"); + config->set_property("Tracking_1G.dump", "false"); + config->set_property("Tracking_1G.if", "0.0"); + config->set_property("Tracking_1G.dump_filename", "./tracking_ch_"); + config->set_property("Tracking_1G.implementation", "GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking"); + config->set_property("Tracking_1G.early_late_space_chips", "0.5"); + config->set_property("Tracking_1G.order", "2"); + config->set_property("Tracking_1G.pll_bw_hz", "2"); + config->set_property("Tracking_1G.dll_bw_hz", "0.5"); +} + +TEST_F(GlonassL1CaDllPllCAidTrackingTest, ValidationOfResults) +{ + struct timeval tv; + long long int begin = 0; + long long int end = 0; + int fs_in = 6625000; + int nsamples = fs_in*4e-3*2; + + init(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Tracking test"); + std::shared_ptr tracking = std::make_shared(config.get(), "Tracking_1G", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaDllPllCAidTrackingTest_msg_rx_make(); + + gnss_synchro.Acq_delay_samples = 1343; + gnss_synchro.Acq_doppler_hz = -2750; + // gnss_synchro.Acq_doppler_hz = -2750; + gnss_synchro.Acq_samplestamp_samples = 0; + + ASSERT_NO_THROW( { + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel." << std::endl; + + ASSERT_NO_THROW( { + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro." << std::endl; + + ASSERT_NO_THROW( { + tracking->connect(top_block); + }) << "Failure connecting tracking to the top_block." << std::endl; + + ASSERT_NO_THROW( { + gr::analog::sig_source_c::sptr sin_source = gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0)); + std::string path = std::string(TEST_PATH); + std::string file = path + "signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin"; + 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); + 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(file_source, 0, valve, 0); + top_block->connect(valve, 0, tracking->get_left_block(), 0); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of tracking test." << std::endl; + + tracking->start_tracking(); + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec *1000000 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec *1000000 + tv.tv_usec; + }) << "Failure running the top_block." << std::endl; + + // TODO: Verify tracking results + std::cout << "Tracked " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; +} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc new file mode 100644 index 000000000..0957b3b24 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc @@ -0,0 +1,209 @@ +/*! + * \file glonass_l1_ca_dll_pll_tracking_test.cc + * \brief This class implements a tracking test for GLONASS_L1_CA_DLL_PLL_Tracking + * implementation based on some input parameters. + * \author Gabriel Araujo, 2017. gabriel.araujo.5000(at)gmail.com + * \author Luis Esteve, 2017. luis(at)epsilon-formacion.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gnss_block_factory.h" +#include "gnss_block_interface.h" +#include "tracking_interface.h" +#include "in_memory_configuration.h" +#include "gnss_sdr_valve.h" +#include "gnss_synchro.h" +#include "glonass_l1_ca_dll_pll_tracking.h" + + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class GlonassL1CaDllPllTrackingTest_msg_rx; + +typedef boost::shared_ptr GlonassL1CaDllPllTrackingTest_msg_rx_sptr; + +GlonassL1CaDllPllTrackingTest_msg_rx_sptr GlonassL1CaDllPllTrackingTest_msg_rx_make(); + +class GlonassL1CaDllPllTrackingTest_msg_rx : public gr::block +{ +private: + friend GlonassL1CaDllPllTrackingTest_msg_rx_sptr GlonassL1CaDllPllTrackingTest_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + GlonassL1CaDllPllTrackingTest_msg_rx(); + +public: + int rx_message; + ~GlonassL1CaDllPllTrackingTest_msg_rx(); //!< Default destructor + +}; + +GlonassL1CaDllPllTrackingTest_msg_rx_sptr GlonassL1CaDllPllTrackingTest_msg_rx_make() +{ + return GlonassL1CaDllPllTrackingTest_msg_rx_sptr(new GlonassL1CaDllPllTrackingTest_msg_rx()); +} + +void GlonassL1CaDllPllTrackingTest_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + +GlonassL1CaDllPllTrackingTest_msg_rx::GlonassL1CaDllPllTrackingTest_msg_rx() : + gr::block("GlonassL1CaDllPllTrackingTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&GlonassL1CaDllPllTrackingTest_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + +GlonassL1CaDllPllTrackingTest_msg_rx::~GlonassL1CaDllPllTrackingTest_msg_rx() +{} + + +// ########################################################### + + +class GlonassL1CaDllPllTrackingTest: public ::testing::Test +{ +protected: + GlonassL1CaDllPllTrackingTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro = Gnss_Synchro(); + } + + ~GlonassL1CaDllPllTrackingTest() + {} + + 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; +}; + + +void GlonassL1CaDllPllTrackingTest::init() +{ + gnss_synchro.Channel_ID = 0; + gnss_synchro.System = 'R'; + std::string signal = "1G"; + signal.copy(gnss_synchro.Signal, 2, 0); + gnss_synchro.PRN = 11; + + config->set_property("GNSS-SDR.internal_fs_hz", "6625000"); + config->set_property("Tracking_1G.item_type", "gr_complex"); + config->set_property("Tracking_1G.dump", "false"); + config->set_property("Tracking_1G.if", "0.0"); + config->set_property("Tracking_1G.dump_filename", "./tracking_ch_"); + config->set_property("Tracking_1G.implementation", "GLONASS_L1_CA_DLL_PLL_Tracking"); + config->set_property("Tracking_1G.early_late_space_chips", "0.5"); + config->set_property("Tracking_1G.order", "2"); + config->set_property("Tracking_1G.pll_bw_hz", "2"); + config->set_property("Tracking_1G.dll_bw_hz", "0.5"); +} + +TEST_F(GlonassL1CaDllPllTrackingTest, ValidationOfResults) +{ + struct timeval tv; + long long int begin = 0; + long long int end = 0; + int fs_in = 6625000; + int nsamples = fs_in*4e-3*2; + + init(); + queue = gr::msg_queue::make(0); + top_block = gr::make_top_block("Tracking test"); + std::shared_ptr tracking = std::make_shared(config.get(), "Tracking_1G", 1, 1); + boost::shared_ptr msg_rx = GlonassL1CaDllPllTrackingTest_msg_rx_make(); + + gnss_synchro.Acq_delay_samples = 1343; + gnss_synchro.Acq_doppler_hz = -2750; + // gnss_synchro.Acq_doppler_hz = -2750; + gnss_synchro.Acq_samplestamp_samples = 0; + + ASSERT_NO_THROW( { + tracking->set_channel(gnss_synchro.Channel_ID); + }) << "Failure setting channel." << std::endl; + + ASSERT_NO_THROW( { + tracking->set_gnss_synchro(&gnss_synchro); + }) << "Failure setting gnss_synchro." << std::endl; + + ASSERT_NO_THROW( { + tracking->connect(top_block); + }) << "Failure connecting tracking to the top_block." << std::endl; + + ASSERT_NO_THROW( { + gr::analog::sig_source_c::sptr sin_source = gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0)); + std::string path = std::string(TEST_PATH); + std::string file = path + "signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin"; + 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); + 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(file_source, 0, valve, 0); + top_block->connect(valve, 0, tracking->get_left_block(), 0); + top_block->connect(tracking->get_right_block(), 0, sink, 0); + top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + }) << "Failure connecting the blocks of tracking test." << std::endl; + + tracking->start_tracking(); + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec *1000000 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec *1000000 + tv.tv_usec; + }) << "Failure running the top_block." << std::endl; + + // TODO: Verify tracking results + std::cout << "Tracked " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl; +} diff --git a/src/utils/matlab/plot_acq_grid_gsoc_glonass.m b/src/utils/matlab/plot_acq_grid_gsoc_glonass.m new file mode 100644 index 000000000..385a53030 --- /dev/null +++ b/src/utils/matlab/plot_acq_grid_gsoc_glonass.m @@ -0,0 +1,99 @@ +% /*! +% * \file plot_acq_grid_gsoc.m +% * \brief Read GNSS-SDR Acquisition dump binary file using the provided +% function and plot acquisition grid of acquisition statistic of PRN sat +% +% This function analyzes a experiment performed by Luis Esteve in the framework +% of the Google Summer of Code (GSoC) 2012, with the collaboration of Javier Arribas +% and Carles Fernández, related to the extension of GNSS-SDR to Galileo. +% +% * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2011 (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 . +% * +% * ------------------------------------------------------------------------- +% */ + +function plot_acq_grid_gsoc_glonass(sat) + +file=['acquisition_R_1G_sat_' num2str(sat) '_doppler_0.dat']; + +% sampling_freq_Hz=62316000 +sampling_freq_Hz=31.75e6 +Doppler_max_Hz = 10000 +Doppler_min_Hz = -10000 +Doppler_step_Hz = 500 + + +% read files + +x=read_complex_binary (file); + +l_y=length(x); + +Doppler_axes=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz; + +l_x=length(Doppler_axes); + +acq_grid = zeros(l_x,l_y); + +index=0; + +for k=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz + index=index+1; + filename=['acquisition_R_1G_sat_' num2str(sat) '_doppler_' num2str(k) '.dat']; + acq_grid(index,:)=abs(read_complex_binary (filename)); + end + + acq_grid = acq_grid.^2; + +maximum_correlation_peak = max(max(acq_grid)) + +[fila,col]=find(acq_grid==max(max(acq_grid))); + +delay_error_sps = col -1 + +Doppler_error_Hz = Doppler_axes(fila) + +noise_grid=acq_grid; +delay_span=floor(3*sampling_freq_Hz/(0.511e6)); +Doppler_span=floor(500/Doppler_step_Hz); +noise_grid(fila-Doppler_span:fila+Doppler_span,col-delay_span:col+delay_span)=0; + +n=numel(noise_grid)-(2*delay_span+1)*(2*Doppler_span+1); + +noise_floor= sum(sum(noise_grid))/n + +Gain_dbs = 10*log10(maximum_correlation_peak/noise_floor) + + +%% Plot 3D FULL RESOLUTION + + +[X,Y] = meshgrid(Doppler_axes,1:1:l_y); +figure; +surf(X,Y,acq_grid'); + +xlabel('Doppler(Hz)');ylabel('Code Delay(samples)');title(['GLRT statistic of Glonass Parallel Code Phase Search Acquisition. Local replica: L1 cboc PRN ' num2str(sat)]); + + +end