diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index d434178ab..7621bd00c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -44,14 +44,6 @@ #include // for abs, pow, floor #include // for complex -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -166,10 +158,10 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } @@ -221,6 +213,12 @@ void GalileoE1PcpsAmbiguousAcquisitionFpga::set_doppler_step(unsigned int dopple acquisition_fpga_->set_doppler_step(doppler_step_); } +void GalileoE1PcpsAmbiguousAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GalileoE1PcpsAmbiguousAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index 07478f380..97bea5acc 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -24,7 +24,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -32,16 +32,15 @@ #ifndef GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ #define GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ +#include "acq_conf.h" #include "channel_fsm.h" +#include "gnss_synchro.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; + class ConfigurationInterface; /*! @@ -51,13 +50,22 @@ class ConfigurationInterface; class GalileoE1PcpsAmbiguousAcquisitionFpga : public AcquisitionInterface { public: + /*! + * \brief Constructor + */ GalileoE1PcpsAmbiguousAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GalileoE1PcpsAmbiguousAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -71,21 +79,38 @@ public: return "Galileo_E1_PCPS_Ambiguous_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ size_t item_size() override { - size_t item_size = sizeof(lv_16sc_t); - return item_size; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and - * tracking blocks + * tracking blocks */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; @@ -122,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -152,9 +182,20 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set resampler latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; bool acquire_pilot_; @@ -162,6 +203,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; std::string dump_filename_; Gnss_Synchro* gnss_synchro_; std::string role_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index e2a2a21f4..a2b0194d1 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -44,14 +44,6 @@ #include // for abs, pow, floor #include // for complex -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, @@ -169,10 +161,10 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } @@ -224,6 +216,12 @@ void GalileoE5aPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GalileoE5aPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GalileoE5aPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index da9f1e1c3..5326df685 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -24,7 +24,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -34,16 +34,12 @@ #include "channel_fsm.h" +#include "gnss_synchro.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t -#include #include #include #include -class Gnss_Synchro; class ConfigurationInterface; @@ -54,13 +50,22 @@ class ConfigurationInterface; class GalileoE5aPcpsAcquisitionFpga : public AcquisitionInterface { public: + /*! + * \brief Constructor + */ GalileoE5aPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GalileoE5aPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -74,20 +79,38 @@ public: return "Galileo_E5a_Pcps_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - return sizeof(lv_16sc_t); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and - * tracking blocks + * tracking blocks */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; @@ -124,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -162,11 +190,19 @@ public: void stop_acquisition() override; /*! - * \brief Sets the resampler latency to account it in the acquisition code delay estimation + * \brief Set resampler latency */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; std::string item_type_; @@ -178,6 +214,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; unsigned int in_streams_; unsigned int out_streams_; Gnss_Synchro* gnss_synchro_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 998da32a0..d82de9ba8 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -47,16 +47,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 - -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -146,10 +136,10 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } @@ -202,6 +192,12 @@ void GpsL1CaPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GpsL1CaPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GpsL1CaPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index 6b2471636..05256163f 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -38,14 +38,11 @@ #include "channel_fsm.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; + class ConfigurationInterface; /*! @@ -55,13 +52,22 @@ class ConfigurationInterface; class GpsL1CaPcpsAcquisitionFpga : public AcquisitionInterface { public: + /*! + * \brief Constructor + */ GpsL1CaPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GpsL1CaPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -75,15 +81,32 @@ public: return "GPS_L1_CA_PCPS_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - size_t item_size = sizeof(lv_16sc_t); - return item_size; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -126,6 +149,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -156,15 +184,30 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set Resampler Latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; uint32_t channel_; std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index c51cadcc8..23a5d9f1f 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -46,13 +46,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -87,7 +80,6 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( // The FPGA can only use FFT lengths that are a power of two. float nbits = ceilf(log2f((float)code_length)); unsigned int nsamples_total = pow(2, nbits); - unsigned int vector_length = nsamples_total; unsigned int select_queue_Fpga = configuration_->property(role + ".select_queue_Fpga", 0); acq_parameters.select_queue_Fpga = select_queue_Fpga; std::string default_device_name = "/dev/uio0"; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h index d560f04d3..e16419989 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h @@ -156,6 +156,13 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; + static const uint32_t QUANT_BITS_LOCAL_CODE = 16; + static const uint32_t SELECT_LSBits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t SELECT_MSBbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t SELECT_ALL_CODE_BITS = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t SHL_CODE_BITS = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; std::string item_type_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index fbafae22c..6ecd55340 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -47,17 +47,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 - -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - - GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -150,10 +139,10 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } @@ -207,6 +196,12 @@ void GpsL5iPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GpsL5iPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GpsL5iPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index ce9a2485e..4cec033d1 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -27,7 +27,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -37,14 +37,10 @@ #include "channel_fsm.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -54,13 +50,22 @@ class ConfigurationInterface; class GpsL5iPcpsAcquisitionFpga : public AcquisitionInterface { public: + /*! + * \brief Constructor + */ GpsL5iPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GpsL5iPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -74,14 +79,32 @@ public: return "GPS_L5i_PCPS_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - return sizeof(lv_16sc_t); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -124,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -154,9 +182,22 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set resampler latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; std::string item_type_; @@ -164,6 +205,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; std::string dump_filename_; Gnss_Synchro* gnss_synchro_; std::string role_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index c721a924a..b15be13a7 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -40,8 +40,6 @@ #include // for move -#define AQ_DOWNSAMPLING_DELAY 40 // delay due to the downsampling filter in the acquisition - pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_) { return pcps_acquisition_fpga_sptr(new pcps_acquisition_fpga(std::move(conf_))); @@ -60,6 +58,7 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) d_num_doppler_bins = 0U; d_threshold = 0.0; d_doppler_step = 0U; + d_doppler_center = 0U; d_doppler_index = 0U; d_test_statistics = 0.0; d_channel = 0U; @@ -141,7 +140,9 @@ void pcps_acquisition_fpga::send_positive_acquisition() << ", code phase " << d_gnss_synchro->Acq_delay_samples << ", doppler " << d_gnss_synchro->Acq_doppler_hz << ", magnitude " << d_mag - << ", input signal power " << d_input_power; + << ", input signal power " << d_input_power + << ", Assist doppler_center " << d_doppler_center; + //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); @@ -211,13 +212,6 @@ void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t } } - // debug - // if (d_test_statistics > d_threshold) - // { - // printf("firstpeak = %f, secondpeak = %f, test_statistics = %f reported block exp = %d PRN = %d inext = %d, initial_sample = %ld doppler = %d\n", firstpeak, secondpeak, d_test_statistics, (int)total_block_exp, (int)d_gnss_synchro->PRN, (int)indext, (long int)initial_sample, (int)doppler); - // printf("doppler_min = %d doppler_step = %d num_doppler_bins = %d\n", (int)doppler_min, (int)doppler_step, (int)num_doppler_bins); - // } - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_sample_counter = initial_sample; @@ -263,7 +257,7 @@ void pcps_acquisition_fpga::set_active(bool active) acquisition_fpga->write_local_code(); acquisition_fpga->set_block_exp(d_total_block_exp); - acquisition_core(d_num_doppler_bins, d_doppler_step, -d_doppler_max); + acquisition_core(d_num_doppler_bins, d_doppler_step, -d_doppler_max + d_doppler_center); if (!d_make_2_steps) { acquisition_fpga->close_device(); @@ -290,7 +284,7 @@ void pcps_acquisition_fpga::set_active(bool active) while (num_second_acq < d_max_num_acqs) { - acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2); + acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2 + d_doppler_center); if (d_test_statistics > d_threshold) { d_active = false; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index b2933c7f5..c47ebce6a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -44,10 +44,10 @@ #include "channel_fsm.h" #include "fpga_acquisition.h" #include -#include // for lv_16sc_t -#include // for uint32_t -#include // for shared_ptr -#include // for string +#include +#include // for uint32_t +#include // for shared_ptr +#include // for string class Gnss_Synchro; @@ -89,6 +89,9 @@ pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); class pcps_acquisition_fpga { public: + /*! + * \brief Destructor + */ ~pcps_acquisition_fpga() = default; /*! @@ -116,7 +119,6 @@ public: /*! * \brief Sets local code for PCPS acquisition algorithm. - * \param code - Pointer to the PRN code. */ void set_local_code(); @@ -181,6 +183,19 @@ public: acquisition_fpga->set_doppler_step(doppler_step); } + /*! + * \brief Set Doppler center frequency for the grid search. It will refresh the Doppler grid. + * \param doppler_center - Frequency center of the search grid [Hz]. + */ + inline void set_doppler_center(int32_t doppler_center) + { + if (doppler_center != d_doppler_center) + { + DLOG(INFO) << " Doppler assistance for Channel: " << d_channel << " => Doppler: " << doppler_center << "[Hz]"; + d_doppler_center = doppler_center; + } + } + /*! * \brief This function triggers a HW reset of the FPGA PL. */ @@ -194,6 +209,7 @@ private: uint32_t d_doppler_index; uint32_t d_channel; uint32_t d_doppler_step; + int32_t d_doppler_center; uint32_t d_doppler_max; uint32_t d_fft_size; uint32_t d_num_doppler_bins; @@ -216,7 +232,7 @@ private: std::weak_ptr d_channel_fsm; void send_negative_acquisition(); void send_positive_acquisition(); - void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_max); + void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_min); float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); }; diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index 7e74b7d01..dd1a7fe1c 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -44,22 +44,6 @@ #include // for move -// FPGA register parameters -#define PAGE_SIZE 0x10000 // default page size for the multicorrelator memory map -#define RESET_ACQUISITION 2 // command to reset the multicorrelator -#define LAUNCH_ACQUISITION 1 // command to launch the multicorrelator -#define TEST_REG_SANITY_CHECK 0x55AA // value to check the presence of the test register (to detect the hw) -#define LOCAL_CODE_CLEAR_MEM 0x10000000 // command to clear the internal memory of the multicorrelator -#define MEM_LOCAL_CODE_WR_ENABLE 0x0C000000 // command to enable the ENA and WR pins of the internal memory of the multicorrelator -#define POW_2_2 4 // 2^2 (used for the conversion of floating point numbers to integers) -#define POW_2_31 2147483648 // 2^31 (used for the conversion of floating point numbers to integers) - -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - - #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ @@ -135,7 +119,7 @@ void Fpga_Acquisition::open_device() LOG(WARNING) << "Cannot open deviceio" << d_device_name; std::cout << "Acq: cannot open deviceio" << d_device_name << std::endl; } - d_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + d_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE_DEFAULT, PROT_READ | PROT_WRITE, MAP_SHARED, d_fd, 0)); if (d_map_base == reinterpret_cast(-1)) @@ -191,12 +175,6 @@ void Fpga_Acquisition::run_acquisition(void) std::cout << "acquisition module Read failed to retrieve 4 bytes!" << std::endl; std::cout << "acquisition module Interrupt number " << irq_count << std::endl; } - - // nbytes = TEMP_FAILURE_RETRY(write(d_fd, reinterpret_cast(&disable_int), sizeof(int32_t))); - // if (nbytes != sizeof(int32_t)) - // { - // std::cerr << "Error disabling interruptions in the FPGA." << std::endl; - // } } @@ -229,7 +207,7 @@ void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_s void Fpga_Acquisition::configure_acquisition() { - //Fpga_Acquisition::open_device(); + //Fpga_Acquisition::(); d_map_base[0] = d_select_queue; d_map_base[1] = d_vector_length; d_map_base[2] = d_nsamples; @@ -274,22 +252,10 @@ void Fpga_Acquisition::read_acquisition_results(uint32_t *max_index, } -void Fpga_Acquisition::block_samples() -{ - d_map_base[14] = 1; // block the samples -} - - -void Fpga_Acquisition::unblock_samples() -{ - d_map_base[14] = 0; // unblock the samples -} - - void Fpga_Acquisition::close_device() { auto *aux = const_cast(d_map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + if (munmap(static_cast(aux), PAGE_SIZE_DEFAULT) == -1) { std::cout << "Failed to unmap memory uio" << std::endl; } @@ -299,6 +265,7 @@ void Fpga_Acquisition::close_device() void Fpga_Acquisition::reset_acquisition(void) { + //printf("============ resetting the hw now from the acquisition ==============="); d_map_base[8] = RESET_ACQUISITION; // writing a 2 to d_map_base[8] resets the acquisition. This causes a reset of all // the FPGA HW modules including the multicorrelators } diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index 0e8b1926d..33a5a6867 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -45,6 +45,9 @@ class Fpga_Acquisition { public: + /*! + * \brief Constructor + */ Fpga_Acquisition( std::string device_name, uint32_t nsamples, @@ -56,14 +59,29 @@ public: uint32_t *all_fft_codes, uint32_t excludelimit); + /*! + * \brief Destructor + */ ~Fpga_Acquisition() = default; + /*! + * \brief Select the code with the chosen PRN + */ bool set_local_code(uint32_t PRN); + /*! + * \brief Configure the doppler sweep parameters in the FPGA + */ void set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min); + /*! + * \brief Run the acquisition process in the FPGA + */ void run_acquisition(void); + /*! + * \brief Read the results of the acquisition process + */ void read_acquisition_results( uint32_t *max_index, float *firstpeak, @@ -73,10 +91,6 @@ public: uint32_t *doppler_index, uint32_t *total_blk_exp); - void block_samples(); - - void unblock_samples(); - /*! * \brief Set maximum Doppler grid search * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. @@ -101,21 +115,51 @@ public: void reset_acquisition(void); /*! - * \brief read the scaling factor that has been used by the FFT-IFFT + * \brief Read the scaling factor that has been used by the FFT-IFFT */ void read_fpga_total_scale_factor(uint32_t *total_scale_factor, uint32_t *fw_scale_factor); + /*! + * \brief Set the block exponent of the FFT in the FPGA. + */ void set_block_exp(uint32_t total_block_exp); + /*! + * \brief Write the PRN code in the FPGA + */ void write_local_code(void); + /*! + * \brief Write the acquisition parameters into the FPGA + */ void configure_acquisition(void); + /*! + * \brief Open the device driver + */ void open_device(); + /*! + * \brief Close the device driver + */ void close_device(); private: + // FPGA register parameters + static const uint32_t PAGE_SIZE_DEFAULT = 0x10000; // default page size for the multicorrelator memory map + static const uint32_t RESET_ACQUISITION = 2; // command to reset the multicorrelator + static const uint32_t LAUNCH_ACQUISITION = 1; // command to launch the multicorrelator + static const uint32_t TEST_REG_SANITY_CHECK = 0x55AA; // value to check the presence of the test register (to detect the hw) + static const uint32_t LOCAL_CODE_CLEAR_MEM = 0x10000000; // command to clear the internal memory of the multicorrelator + static const uint32_t MEM_LOCAL_CODE_WR_ENABLE = 0x0C000000; // command to enable the ENA and WR pins of the internal memory of the multicorrelator + static const uint32_t POW_2_2 = 4; // 2^2 (used for the conversion of floating point numbers to integers) + static const uint32_t POW_2_31 = 2147483648; // 2^31 (used for the conversion of floating point numbers to integers) + + static const uint32_t SELECT_LSBits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t SELECT_MSBbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t SELECT_ALL_CODE_BITS = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t SHL_CODE_BITS = 65536; // shift left by 10 bits + int64_t d_fs_in; // data related to the hardware module and the driver int32_t d_fd; // driver descriptor diff --git a/src/algorithms/signal_source/libs/fpga_switch.cc b/src/algorithms/signal_source/libs/fpga_switch.cc index 933d3400f..c291b0c48 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.cc +++ b/src/algorithms/signal_source/libs/fpga_switch.cc @@ -40,11 +40,6 @@ #include // for cout, endl #include // for mmap - -// constants -const size_t PAGE_SIZE = 0x10000; -const uint32_t TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; - Fpga_Switch::Fpga_Switch(const std::string &device_name) { if ((d_device_descriptor = open(device_name.c_str(), O_RDWR | O_SYNC)) == -1) diff --git a/src/algorithms/signal_source/libs/fpga_switch.h b/src/algorithms/signal_source/libs/fpga_switch.h index 9a44ca2de..1851b911f 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.h +++ b/src/algorithms/signal_source/libs/fpga_switch.h @@ -39,16 +39,33 @@ #include -#define MAX_LENGTH_DEVICEIO_NAME 50 - +/*! + * \brief Class that controls the switch in the FPGA, which connects the FPGA acquisition and multicorrelator modules to + * either the DMA or the Analog Front-End. + */ class Fpga_Switch { public: + /*! + * \brief Constructor + */ Fpga_Switch(const std::string& device_name); + + /*! + * \brief Destructor + */ ~Fpga_Switch(); + + /*! + * \brief This function configures the switch in th eFPGA + */ void set_switch_position(int32_t switch_position); private: + static const size_t PAGE_SIZE = 0x10000; + static const uint32_t TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; + static const uint32_t MAX_LENGTH_DEVICEIO_NAME = 50; + int d_device_descriptor; // driver descriptor volatile unsigned* d_map_base; // driver memory map diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index db4c429c5..8077aaa5f 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -29,7 +29,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -41,19 +41,9 @@ #include "dll_pll_conf_fpga.h" #include "galileo_e1_signal_processing.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include #include #include -#include // for round -#include // for memcpy -#include // for operator<< - -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( ConfigurationInterface* configuration, const std::string& role, @@ -82,7 +72,6 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( { trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); } - float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); if (FLAGS_pll_bw_hz != 0.0) { @@ -174,31 +163,11 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.system = 'E'; std::array sig_{'1', 'B', '\0'}; std::memcpy(trk_param_fpga.signal, sig_.data(), 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); // FPGA configuration parameters std::string default_device_name = "/dev/uio"; @@ -206,7 +175,6 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 15); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 1; // 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 2; @@ -281,6 +249,9 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = GALILEO_E1_B_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + trk_param_fpga.extended_correlation_in_fpga = false; + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h index 690f0039f..b7e7cd721 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h @@ -29,7 +29,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -39,12 +39,9 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include // for basic_block_sptr, basic_block_sptr -#include // for size_t -#include // for uint32_t -#include // for string +#include + -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -54,32 +51,61 @@ class ConfigurationInterface; class GalileoE1DllPllVemlTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GalileoE1DllPllVemlTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GalileoE1DllPllVemlTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga" + /*! + * \brief Returns "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga" + */ inline std::string implementation() override { return "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return sizeof(int); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -94,14 +120,22 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + /*! + * \brief Start the tracking process in the FPGA + */ void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; private: + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; uint32_t channel_; std::string role_; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 3a4a91176..7a2a30a0e 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -24,7 +24,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -36,21 +36,9 @@ #include "dll_pll_conf_fpga.h" #include "galileo_e5_signal_processing.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include -#include // for gr_complex #include #include -#include // for round -#include -#include // for memcpy -#include - -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( ConfigurationInterface *configuration, const std::string &role, @@ -168,30 +156,11 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.system = 'E'; std::array sig_{'5', 'X', '\0'}; std::memcpy(trk_param_fpga.signal, sig_.data(), 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); d_data_codes = nullptr; @@ -201,7 +170,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 27); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 1; // 0 -> 3 correlators, 1 -> up to 5+1 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 1; @@ -264,6 +232,34 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (d_track_pilot) + { + if (extend_correlation_symbols > 1) + { + if (extend_correlation_symbols <= GALILEO_E5A_I_SECONDARY_CODE_LENGTH) + { + if ((GALILEO_E5A_I_SECONDARY_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + } + } + else + { + if (extend_correlation_symbols % GALILEO_E5A_I_SECONDARY_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + } + } + } + } + //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h index 1950e4630..ec574d412 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h @@ -24,7 +24,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -34,12 +34,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include // for basic_block_sptr -#include // For uint32_t -#include // for size_t #include -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -48,32 +44,61 @@ class ConfigurationInterface; class GalileoE5aDllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GalileoE5aDllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GalileoE5aDllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "Galileo_E5a_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "Galileo_E5a_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "Galileo_E5a_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return sizeof(int); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -87,13 +112,23 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Start the tracking process in the FPGA + */ + void start_tracking() override; + + /*! + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; private: + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; uint32_t channel_; std::string role_; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 46219425f..be266a0dd 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -29,7 +29,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,20 +40,10 @@ #include "display.h" #include "dll_pll_conf_fpga.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include "gps_sdr_signal_processing.h" #include #include #include -#include // for round -#include // for memcpy -#include - -#define NUM_PRNs 32 // total number of PRNs - -// the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( ConfigurationInterface* configuration, const std::string& role, @@ -61,7 +51,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( { Dll_Pll_Conf_Fpga trk_param_fpga = Dll_Pll_Conf_Fpga(); DLOG(INFO) << "role " << role; - //################# CONFIGURATION PARAMETERS ######################## int32_t fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); int32_t fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); @@ -76,7 +65,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( { trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); } - bool dump = configuration->property(role + ".dump", false); trk_param_fpga.dump = dump; std::string default_dump_filename = "./track_ch"; @@ -155,9 +143,9 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( symbols_extended_correlator = 1; std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; } - else if (symbols_extended_correlator > 20) + else if (symbols_extended_correlator > GPS_CA_BIT_DURATION_MS) { - symbols_extended_correlator = 20; + symbols_extended_correlator = GPS_CA_BIT_DURATION_MS; std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } trk_param_fpga.extend_correlation_symbols = symbols_extended_correlator; @@ -176,30 +164,11 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.system = 'G'; std::array sig_{'1', 'C', '\0'}; std::memcpy(trk_param_fpga.signal, sig_.data(), 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 30); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.80); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); // FPGA configuration parameters std::string default_device_name = "/dev/uio"; @@ -207,7 +176,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 3); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int32_t), volk_gnsssdr_get_alignment())); @@ -231,6 +199,22 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = 1; // 1 sample per chip + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (symbols_extended_correlator > 1) + { + if (symbols_extended_correlator <= GPS_CA_BIT_DURATION_MS) + { + if ((GPS_CA_BIT_DURATION_MS % symbols_extended_correlator) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = symbols_extended_correlator; + } + } + } + + //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index 680c903f2..ba08f0b76 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -29,7 +29,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -39,12 +39,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include -#include #include - -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -53,32 +49,61 @@ class ConfigurationInterface; class GpsL1CaDllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GpsL1CaDllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GpsL1CaDllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "GPS_L1_CA_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "GPS_L1_CA_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "GPS_L1_CA_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return sizeof(int); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -92,14 +117,24 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + /*! + * \brief Start the tracking process in the FPGA + */ void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; private: + static const uint32_t NUM_PRNs = 32; // total number of PRNs + static const int32_t GPS_CA_BIT_DURATION_MS = 20; + // the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; uint32_t channel_; std::string role_; diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index 387da089b..3c48a2326 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -50,9 +50,6 @@ #include // for memcpy #include -#define NUM_PRNs 32 - - GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) @@ -100,18 +97,18 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( trk_param_fpga.system = 'G'; std::array sig_{'2', 'S', '\0'}; std::memcpy(trk_param_fpga.signal, sig_.data(), 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; - trk_param_fpga.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; - trk_param_fpga.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + + // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + // if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + // trk_param_fpga.max_lock_fail = max_lock_fail; // FPGA configuration parameters std::string default_device_name = "/dev/uio"; @@ -119,14 +116,12 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( trk_param_fpga.device_name = device_name; unsigned int device_base = configuration->property(role + ".device_base", 1); trk_param_fpga.device_base = device_base; - //unsigned int multicorr_type = configuration->property(role + ".multicorr_type", 0); - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators auto* ca_codes_f = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); //################# PRE-COMPUTE ALL THE CODES ################# d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); - for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) + for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l2c_m_code_gen_float(gsl::span(ca_codes_f, static_cast(GPS_L2_M_CODE_LENGTH_CHIPS)), PRN); for (unsigned int s = 0; s < 2 * static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); s++) diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h index 85dcc4707..96223a816 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h @@ -99,6 +99,7 @@ public: void stop_tracking() override; private: + static const uint32_t NUM_PRNs = 32; dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; unsigned int channel_; std::string role_; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 9baf9e8f1..cce524561 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -30,7 +30,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -42,22 +42,10 @@ #include "display.h" #include "dll_pll_conf_fpga.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include "gps_l5_signal.h" #include #include #include -#include // for round -#include // for memcpy -#include - -#define NUM_PRNs 32 // number of PRNS - -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( ConfigurationInterface *configuration, const std::string &role, @@ -175,30 +163,11 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.system = 'G'; std::array sig_{'L', '5', '\0'}; std::memcpy(trk_param_fpga.signal, sig_.data(), 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); // FPGA configuration parameters std::string default_device_name = "/dev/uio"; @@ -206,7 +175,6 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 27); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 1; auto code_length_chips = static_cast(GPS_L5I_CODE_LENGTH_CHIPS); @@ -287,6 +255,34 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (d_track_pilot) + { + if (extend_correlation_symbols > 1) + { + if (extend_correlation_symbols <= GPS_L5I_NH_CODE_LENGTH) + { + if ((GPS_L5I_NH_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + } + } + else + { + if (extend_correlation_symbols % GPS_L5I_NH_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GPS_L5I_NH_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GPS_L5I_NH_CODE_LENGTH; + } + } + } + } + tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; DLOG(INFO) << "tracking(" << tracking_fpga_sc->unique_id() << ")"; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h index 149fa107a..b3bef53ce 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h @@ -30,7 +30,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,12 +40,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include -#include -#include #include -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -54,32 +50,61 @@ class ConfigurationInterface; class GpsL5DllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GpsL5DllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GpsL5DllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "GPS_L5_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "GPS_L5_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "GPS_L5_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return sizeof(int); + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -93,14 +118,24 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + /*! + * \brief Start the tracking process in the FPGA + */ void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; private: + static const uint32_t NUM_PRNs = 32; // total number of PRNs + + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; uint32_t channel_; std::string role_; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index cad74581f..3801b3cdb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -41,24 +41,25 @@ #include "Galileo_E5a.h" #include "MATH_CONSTANTS.h" #include "fpga_multicorrelator.h" -#include "gnss_satellite.h" #include "gnss_sdr_create_directory.h" #include "gnss_synchro.h" +#include "gps_sdr_signal_processing.h" #include "lock_detectors.h" #include "tracking_discriminators.h" #include -#include -#include -#include // for mp +#include // for io_signature +#include // for scoped_lock +#include // for Mat_VarCreate +#include // for mp #include -#include -#include -#include -#include -#include // for abs, size_t -#include -#include +#include // for fill_n +#include // for fmod, round, floor +#include // for exception +#include +#include // for cout, cerr #include +#include +#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -73,7 +74,6 @@ namespace fs = std::filesystem; namespace fs = boost::filesystem; #endif - dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) { return dll_pll_veml_tracking_fpga_sptr(new dll_pll_veml_tracking_fpga(conf_)); @@ -83,26 +83,27 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) : gr::block("dll_pll_veml_tracking_fpga", gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); this->set_relative_rate(1.0 / static_cast(trk_parameters.vector_length)); - // Telemetry bit synchronization message port input (mainly for GPS L1 CA) - this->message_port_register_in(pmt::mp("preamble_samplestamp")); // Telemetry message port input this->message_port_register_in(pmt::mp("telemetry_to_trk")); this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars + d_dll_filt_history.set_capacity(1000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; - d_preambles_symbols = nullptr; - d_preamble_length_symbols = 0; + d_data_secondary_code_length = 0U; + d_data_secondary_code_string = nullptr; signal_type = std::string(trk_parameters.signal); std::map map_signal_pretty_name; @@ -118,6 +119,12 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_samples_per_chip = trk_parameters.code_samples_per_chip; // number of samples per chip d_code_length_chips = trk_parameters.code_length_chips; + d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; + d_current_extended_correlation_in_fpga = false; + d_extend_fpga_integration_periods = trk_parameters.extend_fpga_integration_periods; // by default + d_fpga_integration_period = trk_parameters.fpga_integration_period; // by default + d_current_fpga_integration_period = 1; + d_sc_demodulate_enabled = false; if (trk_parameters.system == 'G') { @@ -127,71 +134,62 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_signal_carrier_freq = GPS_L1_FREQ_HZ; d_code_period = GPS_L1_CA_CODE_PERIOD; d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; - d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - - // set the preamble - uint16_t preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE; - - // preamble bits to sampled symbols - d_preamble_length_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - for (uint16_t preambles_bit : preambles_bits) - { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit + // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization + d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; } else if (signal_type == "2S") { d_signal_carrier_freq = GPS_L2_FREQ_HZ; d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; } else if (signal_type == "L5") { d_signal_carrier_freq = GPS_L5_FREQ_HZ; d_code_period = GPS_L5I_PERIOD; d_code_chip_rate = GPS_L5I_CODE_RATE_HZ; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; d_secondary = true; + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + d_sc_demodulate_enabled = true; + } + } if (trk_parameters.track_pilot) { + // synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; } else { + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -200,7 +198,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -214,6 +211,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_signal_carrier_freq = GALILEO_E1_FREQ_HZ; d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_veml = true; @@ -229,7 +227,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_secondary = false; signal_pretty_name = signal_pretty_name + "B"; } - interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. } else if (signal_type == "5X") { @@ -238,20 +236,31 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ; d_symbols_per_bit = 20; d_correlation_length_ms = 1; - + d_secondary = true; + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + d_sc_demodulate_enabled = true; + } + } if (trk_parameters.track_pilot) { - d_secondary = true; + // synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; + // remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + + // the pilot secondary code depends on PRN and it is initialized later } else { - //Do not acquire secondary code in data component. It is done in telemetry decoder - d_secondary = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -260,7 +269,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -272,7 +280,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -284,9 +291,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & // Initialize tracking ========================================== d_code_loop_filter = Tracking_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false); - //printf("trk_parameters.fll_bw_hz = %f trk_parameters.pll_bw_hz = %f trk_parameters.pll_filter_order = %d\n", trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); + // correlator outputs (scalar) if (d_veml) { // Very-Early, Early, Prompt, Late, Very-Late @@ -345,33 +352,38 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; + d_rem_code_phase_samples_prev = 0.0; // previously calculated d_rem_code_phase_samples // Residual carrier phase d_rem_carr_phase_rad = 0.0; // sample synchronization d_sample_counter = 0ULL; d_acq_sample_stamp = 0ULL; - d_absolute_samples_offset = 0ULL; - d_current_prn_length_samples = static_cast(trk_parameters.vector_length); - d_next_prn_length_samples = d_current_prn_length_samples; + d_current_integration_length_samples = static_cast(trk_parameters.vector_length); + d_next_integration_length_samples = d_current_integration_length_samples; d_current_correlation_time_s = 0.0; - d_correlation_length_samples = static_cast(trk_parameters.vector_length); // this one is only for initialisation and does not change its value (MM) - // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; d_Prompt_buffer = std::vector(trk_parameters.cn0_samples); d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_carrier_lock_threshold = trk_parameters.carrier_lock_th; + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_cn0_smoother = Exponential_Smoother(); if (d_code_period > 0.0) { d_cn0_smoother.set_samples_for_initialization(200 / static_cast(d_code_period * 1000.0)); } - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = trk_parameters.carrier_lock_th; - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + + d_carrier_lock_test_smoother = Exponential_Smoother(); + d_carrier_lock_test_smoother.set_alpha(0.002); + d_carrier_lock_test_smoother.set_min_value(-1.0); + d_carrier_lock_test_smoother.set_offset(0.0); + d_carrier_lock_test_smoother.set_samples_for_initialization(25); d_acquisition_gnss_synchro = nullptr; d_channel = 0; @@ -386,9 +398,8 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_carrier_phase_step_rad = 0.0; d_carrier_phase_rate_step_rad = 0.0; d_rem_code_phase_chips = 0.0; - d_state = 0; // initial state: standby + d_state = 1; // initial state: standby clear_tracking_vars(); - if (trk_parameters.smoother_length > 0) { d_carr_ph_history.set_capacity(trk_parameters.smoother_length * 2); @@ -440,10 +451,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & uint32_t device_base = trk_parameters.device_base; int32_t *ca_codes = trk_parameters.ca_codes; int32_t *data_codes = trk_parameters.data_codes; - uint32_t multicorr_type = trk_parameters.multicorr_type; - multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, multicorr_type, d_code_samples_per_chip); + multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, d_code_samples_per_chip); multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data); d_sample_counter_next = 0ULL; + + d_corrected_doppler = false; + + d_worker_is_done = false; + + d_stop_tracking = false; } @@ -460,7 +476,7 @@ void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t & { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); - d_carrier_lock_fail_counter = 10000; // force loss-of-lock condition + d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition } } } @@ -491,18 +507,15 @@ void dll_pll_veml_tracking_fpga::start_tracking() d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter - // enable tracking pull-in - d_state = 1; -} + d_corrected_doppler = false; + boost::mutex::scoped_lock lock(d_mutex); + d_worker_is_done = true; + m_condition.notify_one(); +} dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() { - if (signal_type == "1C") - { - volk_gnsssdr_free(d_preambles_symbols); - } - if (d_dump_file.is_open()) { try @@ -546,7 +559,6 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() for (uint32_t i = 0; i < d_secondary_code_length; i++) { if (d_Prompt_circular_buffer[i].real() < 0.0) // symbols clipping - //if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping { if (d_secondary_code_string->at(i) == '0') { @@ -585,47 +597,57 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra if (d_cn0_estimation_counter < trk_parameters.cn0_samples) { // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; - d_cn0_estimation_counter++; + d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; return true; } - else + d_cn0_estimation_counter = 0; + d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; + // Code lock indicator + float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); + // Carrier lock indicator + //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + // Loss of lock detection + if (!d_pull_in_transitory) { - d_cn0_estimation_counter = 0; - // Code lock indicator - float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); - d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples); - // Loss of lock detection - if (!d_pull_in_transitory) + if (d_carrier_lock_test < d_carrier_lock_threshold) { - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) - { - d_carrier_lock_fail_counter--; - } - } - } - if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - multicorrelator_fpga->unlock_channel(); - return false; + d_carrier_lock_fail_counter++; } else { - return true; + if (d_carrier_lock_fail_counter > 0) + { + d_carrier_lock_fail_counter--; + } + } + + if (d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + { + d_code_lock_fail_counter++; + } + else + { + if (d_code_lock_fail_counter > 0) + { + d_code_lock_fail_counter--; + } } } + if (d_carrier_lock_fail_counter > trk_parameters.max_carrier_lock_fail or d_code_lock_fail_counter > trk_parameters.max_code_lock_fail) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel + << " (carrier_lock_fail_counter:" << d_carrier_lock_fail_counter + << " code_lock_fail_counter : " << d_code_lock_fail_counter << ")"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + multicorrelator_fpga->unlock_channel(); + return false; + } + return true; } @@ -645,7 +667,7 @@ void dll_pll_veml_tracking_fpga::do_correlation_step(void) static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), - d_current_prn_length_samples); + d_current_integration_length_samples); } @@ -653,7 +675,6 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() { // ################## PLL ########################################################## // PLL discriminator - //printf("d_cloop = %d\n", d_cloop); if (d_cloop) { // Costas loop discriminator, insensitive to 180 deg phase transitions @@ -668,17 +689,20 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) { // FLL discriminator - d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + // d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_P_accu_old = d_P_accu; + // std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; // Carrier discriminator filter if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) { - //pure FLL, disable PLL + // pure FLL, disable PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); } else { - //FLL-aided PLL + // FLL-aided PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); } } @@ -693,6 +717,7 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; + // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -705,9 +730,30 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] - // New code Doppler frequency estimation d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; + + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler + if (trk_parameters.enable_doppler_correction == true) + { + if (d_pull_in_transitory == false and d_corrected_doppler == false) + { + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + + if (d_dll_filt_history.full()) + { + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); + LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); + d_corrected_doppler = true; + } + d_dll_filt_history.clear(); + } + } + } } @@ -717,6 +763,7 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() if (trk_parameters.track_pilot) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); } d_P_accu_old = gr_complex(0.0, 0.0); d_carr_phase_error_hz = 0.0; @@ -725,6 +772,7 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; + d_current_data_symbol = 0; d_Prompt_circular_buffer.clear(); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -741,20 +789,21 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - //d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples - d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples - //int32_t actual_prn_length_samples = static_cast(std::floor(K_blk_samples)); - //d_next_prn_length_samples = actual_prn_length_samples + (actual_prn_length_samples - d_current_prn_length_samples); + //K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function + K_blk_samples = T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function + auto actual_blk_length = static_cast(std::floor(K_blk_samples)); + //d_next_integration_length_samples = 2 * actual_blk_length - d_current_integration_length_samples; + d_next_integration_length_samples = actual_blk_length; //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] if (trk_parameters.high_dyn) { - d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_prn_length_samples))); + d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_integration_length_samples))); if (d_carr_ph_history.full()) { double tmp_cp1 = 0.0; @@ -771,23 +820,23 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator - //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; - d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; + d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; if (trk_parameters.high_dyn) { - d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_prn_length_samples))); + d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_integration_length_samples))); if (d_code_ph_history.full()) { double tmp_cp1 = 0.0; @@ -805,14 +854,15 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() } } // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_samples_prev = d_rem_code_phase_samples; + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; } void dll_pll_veml_tracking_fpga::save_correlation_results() { - if (d_secondary) + if (d_secondary && (!d_current_extended_correlation_in_fpga)) // the FPGA removes the secondary code { if (d_secondary_code_string->at(d_current_symbol) == '0') { @@ -850,12 +900,83 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; - d_current_symbol++; - d_current_symbol %= d_symbols_per_bit; } - // If tracking pilot, disable Costas loop + + // data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (trk_parameters.track_pilot) + { + if (!d_current_extended_correlation_in_fpga) // the FPGA removes the secondary code + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + d_P_data_accu += *d_Prompt_Data; + } + } + else + { + if (!d_current_extended_correlation_in_fpga) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + else + { + d_P_data_accu += *d_Prompt; + } + } + + d_current_data_symbol += d_current_fpga_integration_period; + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu += *d_Prompt; + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + } + d_current_data_symbol += d_current_fpga_integration_period; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu = *d_Prompt_Data; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + if (trk_parameters.track_pilot) { + // If tracking pilot, disable Costas loop d_cloop = false; } else @@ -865,7 +986,7 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() } -void dll_pll_veml_tracking_fpga::log_data(bool integrating) +void dll_pll_veml_tracking_fpga::log_data() { if (d_dump) { @@ -878,29 +999,13 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) uint64_t tmp_long_int; if (trk_parameters.track_pilot) { - if (interchange_iq) - { - prompt_I = d_Prompt_Data->imag(); - prompt_Q = d_Prompt_Data->real(); - } - else - { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); - } + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); } else { - if (interchange_iq) - { - prompt_I = d_Prompt->imag(); - prompt_Q = d_Prompt->real(); - } - else - { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); - } + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); } if (d_veml) { @@ -915,20 +1020,6 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); - if (integrating) - { - //TODO: Improve this solution! - // It compensates the amplitude difference while integrating - if (d_extend_correlation_symbols_count > 0) - { - float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; - } - } try { @@ -942,7 +1033,7 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + tmp_long_int = d_sample_counter_next; d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); // accumulated carrier phase tmp_float = d_acc_carrier_phase_rad; @@ -960,7 +1051,6 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands tmp_float = d_carr_phase_error_hz; - //tmp_float = d_carr_error_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carr_error_filt_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); @@ -977,7 +1067,7 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(d_sample_counter_next); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -1050,7 +1140,6 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() auto aux1 = std::vector(num_epoch); auto aux2 = std::vector(num_epoch); auto PRN = std::vector(num_epoch); - try { if (dump_file.is_open()) @@ -1194,6 +1283,8 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { + gr::thread::scoped_lock l(d_setlock); + d_channel = channel; multicorrelator_fpga->set_channel(d_channel); LOG(INFO) << "Tracking Channel set to " << d_channel; @@ -1222,6 +1313,42 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) } } } + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + // Now we can write the secondary codes that do not depend on the PRN number + if (trk_parameters.system == 'G') + { + if (signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); + } + else + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, 0); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + } + } + } + else if (trk_parameters.system == 'E') + { + if (signal_type == "5X") + { + // coherent integration in the FPGA is only enabled when tracking the pilot. + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); + } + } + } + } + } } @@ -1230,15 +1357,15 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_acquisition_gnss_synchro = p_gnss_synchro; if (p_gnss_synchro->PRN > 0) { - // When using the FPGA the SW only reads the sample counter during active tracking in order to spare CPU clock cycles. + // A set_gnss_synchro command with a valid PRN is received when the system is going to run acquisition with that PRN. + // We can use this command to pre-initialize tracking parameters and variables before the actual acquisition process takes place. + // In this way we minimize the latency between acquisition and tracking once the acquisition has been made. d_sample_counter = 0; d_sample_counter_next = 0; d_carrier_phase_rate_step_rad = 0.0; d_code_ph_history.clear(); - - d_carr_ph_history.clear(); if (systemName == "GPS" and signal_type == "L5") @@ -1260,13 +1387,23 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + } + } } } std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; d_rem_code_phase_samples = 0.0; d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; @@ -1275,6 +1412,8 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; + d_code_phase_rate_step_chips = 0.0; + if (d_veml) { d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); @@ -1303,18 +1442,35 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_cloop = true; d_Prompt_circular_buffer.clear(); + + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + + // re-establish nominal integration length (not extended integration by default) + d_current_integration_length_samples = static_cast(trk_parameters.vector_length); + d_next_integration_length_samples = d_current_integration_length_samples; + + multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default + + d_current_fpga_integration_period = 1; + d_current_extended_correlation_in_fpga = false; + + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); } } void dll_pll_veml_tracking_fpga::stop_tracking() { - d_state = 0; + // interrupt the tracking loops + d_stop_tracking = true; } void dll_pll_veml_tracking_fpga::reset(void) { + gr::thread::scoped_lock l(d_setlock); multicorrelator_fpga->unlock_channel(); } @@ -1324,423 +1480,489 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_void_star &output_items) { + gr::thread::scoped_lock l(d_setlock); auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; - d_current_prn_length_samples = d_next_prn_length_samples; - - - if (d_pull_in_transitory == true) + while ((!current_synchro_data.Flag_valid_symbol_output) && (!d_stop_tracking)) { - if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking + d_current_integration_length_samples = d_next_integration_length_samples; + + if (d_pull_in_transitory == true) { - if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking { - d_pull_in_transitory = false; + if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + } } } - } - switch (d_state) - { - case 0: // Standby - Consume samples at full throttle, do nothing - { - *out[0] = *d_acquisition_gnss_synchro; - usleep(1000); - return 1; - } - case 1: // Pull-in - { - // Signal alignment (skip samples until the incoming signal is aligned with local replica) - - int64_t acq_trk_diff_samples; - double acq_trk_diff_seconds; - double delta_trk_to_acq_prn_start_samples; - - multicorrelator_fpga->lock_channel(); - uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); - uint64_t absolute_samples_offset; - if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) + switch (d_state) + { + case 1: // Pull-in { + d_worker_is_done = false; + + + boost::mutex::scoped_lock lock(d_mutex); + while (!d_worker_is_done) m_condition.wait(lock); + // Signal alignment (skip samples until the incoming signal is aligned with local replica) - acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; - uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_correlation_length_samples); - absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_correlation_length_samples); - } - else - { - // test mode + int64_t acq_trk_diff_samples; + double acq_trk_diff_seconds; + double delta_trk_to_acq_prn_start_samples; + uint64_t absolute_samples_offset; - acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; - - absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); - } - multicorrelator_fpga->set_initial_sample(absolute_samples_offset); - d_absolute_samples_offset = absolute_samples_offset; - d_sample_counter = absolute_samples_offset; - current_synchro_data.Tracking_sample_counter = absolute_samples_offset; - d_sample_counter_next = d_sample_counter; - - // Doppler effect Fd = (C / (C + Vr)) * F - double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; - // new chip and PRN sequence periods based on acq Doppler - d_code_freq_chips = radial_velocity * d_code_chip_rate; - d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - d_code_phase_rate_step_chips = 0.0; - //double T_chip_mod_seconds = 1.0 / d_code_freq_chips; - //double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); - //double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in; - - d_acq_code_phase_samples = absolute_samples_offset; - - //d_current_prn_length_samples = round(T_prn_mod_samples); - d_current_prn_length_samples = trk_parameters.vector_length; - - d_next_prn_length_samples = d_current_prn_length_samples; - int32_t samples_offset = round(d_acq_code_phase_samples); - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); - d_state = 2; - d_cn0_smoother.reset(); - - // DEBUG OUTPUT - std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; - std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; - DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz - << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - - *out[0] = *d_acquisition_gnss_synchro; - return 1; - } - case 2: // Wide tracking and symbol synchronization - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); - - do_correlation_step(); - - // Save single correlation step variables - - if (d_veml) - { - d_VE_accu = *d_Very_Early; - d_VL_accu = *d_Very_Late; - } - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - - // Check lock status - - if (!cn0_and_tracking_lock_status(d_code_period)) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - bool next_state = false; - // Perform DLL/PLL tracking loop computations. Costas Loop enabled - - run_dll_pll(); - - update_tracking_vars(); - - // enable write dump file this cycle (valid DLL/PLL cycle) - - log_data(false); - - if (d_secondary) + multicorrelator_fpga->lock_channel(); + uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); + if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_circular_buffer.push_back(*d_Prompt); + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); - - if (next_state) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - } + uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples); + absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples); } - else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + else { - float current_tracking_time_s = static_cast(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in; + // test mode - if (current_tracking_time_s > 10) + acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; + + absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); + } + + + multicorrelator_fpga->set_initial_sample(absolute_samples_offset); + //d_absolute_samples_offset = absolute_samples_offset; + d_sample_counter = absolute_samples_offset; + d_sample_counter_next = d_sample_counter; + + // Doppler effect Fd = (C / (C + Vr)) * F + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; + // new chip and PRN sequence periods based on acq Doppler + d_code_freq_chips = radial_velocity * d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + + d_acq_code_phase_samples = absolute_samples_offset; + + int32_t samples_offset = round(d_acq_code_phase_samples); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + + d_state = 2; + + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + // DEBUG OUTPUT + std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + // std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; + // DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + // << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + break; + } + case 2: // Wide tracking and symbol synchronization + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + do_correlation_step(); + + // Save single correlation step variables + + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + //fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period + if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + + // Check lock status + + if (!cn0_and_tracking_lock_status(d_code_period)) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + } + else + { + bool next_state = false; + + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); + + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + if (!d_pull_in_transitory) { - d_symbol_history.push_back(d_Prompt->real()); - - //******* preamble correlation ******** - - int32_t corr_value = 0; - if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) + if (d_secondary) { - int i = 0; - for (const auto &iter : d_symbol_history) + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) { - if (iter < 0.0) // symbols clipping + next_state = acquire_secondary(); + + if (next_state) { - corr_value -= d_preambles_symbols[i]; + LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; } - else - { - corr_value += d_preambles_symbols[i]; - } - i++; } } - - if (corr_value == d_preamble_length_symbols) + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - next_state = true; + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } } else { - next_state = false; + next_state = true; } } else { - next_state = false; + next_state = false; //keep in state 2 during pull-in transitory } + + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_Prompt_circular_buffer.clear(); + d_current_symbol = 0; + d_current_data_symbol = 0; + + if (d_enable_extended_integration) + { + // update integration time + d_extend_correlation_symbols_count = 0; + d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + + if (d_extended_correlation_in_fpga) + { + d_current_fpga_integration_period = d_fpga_integration_period; + d_current_extended_correlation_in_fpga = true; + + d_P_accu_old.real(d_P_accu_old.real() * d_fpga_integration_period); + d_P_accu_old.imag(d_P_accu_old.imag() * d_fpga_integration_period); + + if (d_sc_demodulate_enabled) + { + multicorrelator_fpga->enable_secondary_codes(); + } + + if (d_extend_fpga_integration_periods > 1) + { + // correction on already computed parameters + K_blk_samples = T_prn_samples * (d_fpga_integration_period) + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 5; + } + else + { + // correction on already computed parameters + K_blk_samples = T_prn_samples * trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 6; + } + } + else + { + d_state = 3; // next state is the extended correlator integrator + } + + LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_update_interval(d_current_correlation_time_s); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + } + } + else + { + d_state = 4; + } + } + } + + break; + } + case 3: // coherent integration (correlation time extension) + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; } else { - next_state = true; - } + run_dll_pll(); + update_tracking_vars(); - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) + if (d_current_data_symbol == 0) { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - - if (next_state) - { // reset extended correlator + // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); d_P_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); - d_Prompt_circular_buffer.clear(); - d_current_symbol = 0; - if (d_enable_extended_integration) { - // UPDATE INTEGRATION TIME - d_extend_correlation_symbols_count = 0; - d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - - d_state = 3; // next state is the extended correlator integrator - LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_update_interval(d_current_correlation_time_s); - d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); - d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); - if (d_veml) - { - d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - } - } - else - { - d_state = 4; + d_state = 3; // new coherent integration (correlation time extension) cycle } } + break; } - break; - } - case 3: // coherent integration (correlation time extension) - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - // perform a correlation step - do_correlation_step(); - update_tracking_vars(); - save_correlation_results(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + case 5: // coherent integration (correlation time extension) { - if (trk_parameters.track_pilot) + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // this must be computed for the secondary prn code + if (d_secondary) { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - log_data(true); - break; - } - case 4: // narrow tracking - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; + uint32_t first_prn_length = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_prn_length = static_cast(std::floor(T_prn_samples)); - // perform a correlation step - do_correlation_step(); - save_correlation_results(); + multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); + } - // check lock status - if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - run_dll_pll(); + // perform a correlation step + do_correlation_step(); + save_correlation_results(); update_tracking_vars(); - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + if (d_current_data_symbol == 0) { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 6; + } + + break; + } + + + case 6: // narrow tracking IN THE FPGA + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // this must be computed for the secondary prn code + if (d_secondary) + { + uint32_t first_prn_length = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_prn_length = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); + } + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; } else { - if (trk_parameters.track_pilot) + run_dll_pll(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else + + d_extend_correlation_symbols_count = 0; + + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + + if (d_extend_fpga_integration_periods > 1) { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + d_state = 5; } } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - if (d_enable_extended_integration) - { - d_state = 3; // new coherent integration (correlation time extension) cycle - } + break; } - } + } } + if (current_synchro_data.Flag_valid_symbol_output) { current_synchro_data.fs = static_cast(trk_parameters.fs_in); - current_synchro_data.Tracking_sample_counter = d_sample_counter_next; + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; //d_sample_counter; *out[0] = current_synchro_data; return 1; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index bb63e0f89..6602cfc8c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -43,11 +43,8 @@ #include // for gr_vector_int, gr_vector... #include // for pmt_t #include // for int32_t -#include // for deque #include // for string, ofstream -#include // for shared_ptr -#include -#include // for pair +#include // for pair #include class Fpga_Multicorrelator_8sc; @@ -65,23 +62,51 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co class dll_pll_veml_tracking_fpga : public gr::block { public: + /*! + * \brief Destructor + */ ~dll_pll_veml_tracking_fpga(); + /*! + * \brief Set the channel number and configure some multicorrelator parameters + */ void set_channel(uint32_t channel); + + /*! + * \brief This function is used with two purposes: + * 1 -> To set the gnss_synchro + * 2 -> A set_gnss_synchro command with a valid PRN is received when the system is going to run + * acquisition with that PRN. We can use this command to pre-initialize tracking parameters and + * variables before the actual acquisition process takes place. In this way we minimize the + * latency between acquisition and tracking once the acquisition has been made. + */ void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); + + /*! + * \brief This function starts the tracking process + */ void start_tracking(); + + /*! + * \brief This function sets a flag that makes general_work to stop in order to finish the tracking process. + */ void stop_tracking(); + /*! + * \brief General Work + */ int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + /*! + * \brief This function disables the HW multicorrelator in the FPGA in order to stop the tracking process + */ void reset(void); private: friend dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_); void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_); - void msg_handler_preamble_index(pmt::pmt_t msg); bool cn0_and_tracking_lock_status(double coh_integration_time_s); bool acquire_secondary(); @@ -90,11 +115,9 @@ private: void update_tracking_vars(); void clear_tracking_vars(); void save_correlation_results(); - void log_data(bool integrating); + void log_data(); int32_t save_matfile(); - //void run_state_2(Gnss_Synchro ¤t_synchro_data); - // tracking configuration vars Dll_Pll_Conf_Fpga trk_parameters; bool d_veml; @@ -102,25 +125,25 @@ private: uint32_t d_channel; Gnss_Synchro *d_acquisition_gnss_synchro; - //Signal parameters + // Signal parameters bool d_secondary; - bool interchange_iq; double d_signal_carrier_freq; double d_code_period; double d_code_chip_rate; uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; uint32_t d_code_length_chips; uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) int32_t d_symbols_per_bit; std::string systemName; std::string signal_type; std::string *d_secondary_code_string; + std::string *d_data_secondary_code_string; std::string signal_pretty_name; - int32_t *d_preambles_symbols; - int32_t d_preamble_length_symbols; - boost::circular_buffer d_symbol_history; + // dll filter buffer + boost::circular_buffer d_dll_filt_history; // tracking state machine int32_t d_state; @@ -141,6 +164,7 @@ private: bool d_enable_extended_integration; int32_t d_extend_correlation_symbols_count; int32_t d_current_symbol; + int32_t d_current_data_symbol; gr_complex d_VE_accu; gr_complex d_E_accu; @@ -149,6 +173,7 @@ private: gr_complex d_L_accu; gr_complex d_VL_accu; + gr_complex d_P_data_accu; gr_complex *d_Prompt_Data; double d_code_phase_step_chips; @@ -157,11 +182,12 @@ private: double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; boost::circular_buffer> d_carr_ph_history; + // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; + double d_rem_code_phase_samples_prev; float d_rem_carr_phase_rad; - // PLL and DLL filter library Tracking_loop_filter d_code_loop_filter; Tracking_FLL_PLL_filter d_carrier_loop_filter; @@ -171,6 +197,7 @@ private: // tracking vars bool d_pull_in_transitory; + bool d_corrected_doppler; double d_current_correlation_time_s; double d_carr_phase_error_hz; double d_carr_freq_error_hz; @@ -185,36 +212,44 @@ private: double T_prn_seconds; double T_prn_samples; double K_blk_samples; - // PRN period in samples - int32_t d_current_prn_length_samples; + // integration period in samples + int32_t d_current_integration_length_samples; // processing samples counters uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; - uint64_t d_absolute_samples_offset; - // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; int32_t d_carrier_lock_fail_counter; - //std::deque d_carrier_lock_detector_queue; + int32_t d_code_lock_fail_counter; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; - //std::deque d_Prompt_buffer_deque; std::vector d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; - + Exponential_Smoother d_carrier_lock_test_smoother; // file dump std::ofstream d_dump_file; std::string d_dump_filename; bool d_dump; bool d_dump_mat; - // extra - int32_t d_correlation_length_samples; - int32_t d_next_prn_length_samples; + bool d_extended_correlation_in_fpga; + bool d_current_extended_correlation_in_fpga; + int32_t d_next_integration_length_samples; + double d_extended_integration_first_acc_carrier_phase_rad; + double d_extended_integration_next_acc_carrier_phase_rad_step; uint64_t d_sample_counter_next; + bool d_sc_demodulate_enabled; + int32_t d_extend_fpga_integration_periods; + uint32_t d_fpga_integration_period; + uint32_t d_current_fpga_integration_period; + bool d_worker_is_done; + boost::condition_variable m_condition; + boost::mutex d_mutex; + + bool d_stop_tracking; }; #endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 9c4447cf5..0f87846e7 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -32,6 +32,8 @@ #include "dll_pll_conf_fpga.h" +#include "gnss_sdr_flags.h" +#include Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() { @@ -45,7 +47,8 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() dump_filename = std::string("./dll_pll_dump.dat"); enable_fll_pull_in = false; enable_fll_steady_state = false; - pull_in_time_s = 2; + pull_in_time_s = 10; + bit_synchronization_time_limit_s = pull_in_time_s + 60; fll_filter_order = 1; pll_filter_order = 3; dll_filter_order = 2; @@ -61,10 +64,13 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() early_late_space_narrow_chips = 0.1; very_early_late_space_narrow_chips = 0.1; extend_correlation_symbols = 5; - cn0_samples = 20; - cn0_min = 25; - max_lock_fail = 50; - carrier_lock_th = 0.85; + cn0_samples = FLAGS_cn0_samples; + cn0_min = FLAGS_cn0_min; + max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; + max_code_lock_fail = FLAGS_max_lock_fail; + carrier_lock_th = FLAGS_carrier_lock_th; + //max_lock_fail = 50; + enable_doppler_correction = false; track_pilot = false; system = 'G'; signal[0] = '1'; @@ -72,9 +78,11 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() signal[2] = '\0'; device_name = "/dev/uio"; device_base = 1U; - multicorr_type = 0U; code_length_chips = 0U; code_samples_per_chip = 0U; ca_codes = nullptr; data_codes = nullptr; + extended_correlation_in_fpga = false; + extend_fpga_integration_periods = 1; + fpga_integration_period = 0; } diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index a614d32ff..1f1ca112c 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -42,14 +42,15 @@ class Dll_Pll_Conf_Fpga { public: /* DLL/PLL tracking configuration */ + int fll_filter_order; bool enable_fll_pull_in; bool enable_fll_steady_state; unsigned int pull_in_time_s; // signed integer, when pull in time is not yet reached it has to be compared against a negative number + unsigned int bit_synchronization_time_limit_s; int pll_filter_order; int dll_filter_order; - double fs_in; uint32_t vector_length; bool dump; @@ -70,19 +71,24 @@ public: bool high_dyn; int32_t cn0_samples; int32_t cn0_min; - int32_t max_lock_fail; + int32_t max_code_lock_fail; + int32_t max_carrier_lock_fail; + //int32_t max_lock_fail; uint32_t smoother_length; double carrier_lock_th; bool track_pilot; + bool enable_doppler_correction; char system; char signal[3]; std::string device_name; uint32_t device_base; - uint32_t multicorr_type; uint32_t code_length_chips; uint32_t code_samples_per_chip; int32_t* ca_codes; int32_t* data_codes; + bool extended_correlation_in_fpga; + uint32_t extend_fpga_integration_periods; + uint32_t fpga_integration_period; Dll_Pll_Conf_Fpga(); }; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index 9f52ec3f3..4f0b1f06f 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -43,20 +43,6 @@ #include // for PROT_READ, PROT_WRITE, MAP_SHARED #include -// FPGA register access constants -#define PAGE_SIZE 0x10000 -#define MAX_LENGTH_DEVICEIO_NAME 50 -#define CODE_RESAMPLER_NUM_BITS_PRECISION 20 -#define CODE_PHASE_STEP_CHIPS_NUM_NBITS CODE_RESAMPLER_NUM_BITS_PRECISION -#define pwrtwo(x) (1 << (x)) -#define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS -#define PHASE_CARR_MAX 2147483648 // 2^(31) The phase is represented as a 32-bit vector in 1.31 format -#define PHASE_CARR_MAX_div_PI 683565275.5764316 // 2^(31)/pi -#define TWO_PI 6.283185307179586 -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 -#define LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER 0x10000000 -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 -#define TEST_REGISTER_TRACK_WRITEVAL 0x55AA #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ @@ -72,7 +58,8 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, - uint32_t multicorr_type, uint32_t code_samples_per_chip) + uint32_t code_samples_per_chip) + { d_n_correlators = n_correlators; d_device_name = std::move(device_name); @@ -114,10 +101,12 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, d_code_length_chips = code_length_chips; d_ca_codes = ca_codes; d_data_codes = data_codes; - d_multicorr_type = multicorr_type; d_code_samples_per_chip = code_samples_per_chip; d_code_length_samples = d_code_length_chips * d_code_samples_per_chip; + + d_secondary_code_enabled = false; + DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; } @@ -139,8 +128,8 @@ Fpga_Multicorrelator_8sc::~Fpga_Multicorrelator_8sc() uint64_t Fpga_Multicorrelator_8sc::read_sample_counter() { uint64_t sample_counter_tmp, sample_counter_msw_tmp; - sample_counter_tmp = d_map_base[SAMPLE_COUNTER_REG_ADDR_LSW]; - sample_counter_msw_tmp = d_map_base[SAMPLE_COUNTER_REG_ADDR_MSW]; + sample_counter_tmp = d_map_base[sample_counter_reg_addr_lsw]; + sample_counter_msw_tmp = d_map_base[sample_counter_reg_addr_msw]; sample_counter_msw_tmp = sample_counter_msw_tmp << 32; sample_counter_tmp = sample_counter_tmp + sample_counter_msw_tmp; // 2^32 return sample_counter_tmp; @@ -150,8 +139,8 @@ uint64_t Fpga_Multicorrelator_8sc::read_sample_counter() void Fpga_Multicorrelator_8sc::set_initial_sample(uint64_t samples_offset) { d_initial_sample_counter = samples_offset; - d_map_base[INITIAL_COUNTER_VALUE_REG_ADDR_LSW] = (d_initial_sample_counter & 0xFFFFFFFF); - d_map_base[INITIAL_COUNTER_VALUE_REG_ADDR_MSW] = (d_initial_sample_counter >> 32) & 0xFFFFFFFF; + d_map_base[initial_counter_value_reg_addr_lsw] = (d_initial_sample_counter & 0xFFFFFFFF); + d_map_base[initial_counter_value_reg_addr_msw] = (d_initial_sample_counter >> 32) & 0xFFFFFFFF; } @@ -205,6 +194,16 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( std::cout << "Tracking_module Read failed to retrieve 4 bytes!" << std::endl; std::cout << "Tracking_module Interrupt number " << irq_count << std::endl; } + + // release secondary code indices, keep channel locked + if (d_secondary_code_enabled == true) + { + d_map_base[drop_samples_reg_addr] = enable_secondary_code; // keep secondary code enabled + } + else + { + d_map_base[drop_samples_reg_addr] = 0; // block samples + } Fpga_Multicorrelator_8sc::read_tracking_gps_results(); } @@ -233,7 +232,7 @@ bool Fpga_Multicorrelator_8sc::free() void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) { - char device_io_name[MAX_LENGTH_DEVICEIO_NAME]; // driver io name + char device_io_name[max_length_deviceio_name]; // driver io name d_channel = channel; // open the device corresponding to the assigned channel @@ -243,9 +242,9 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) devicebasetemp << numdevice; mergedname = d_device_name + devicebasetemp.str(); - if (mergedname.size() > MAX_LENGTH_DEVICEIO_NAME) + if (mergedname.size() > max_length_deviceio_name) { - mergedname = mergedname.substr(0, MAX_LENGTH_DEVICEIO_NAME); + mergedname = mergedname.substr(0, max_length_deviceio_name); } mergedname.copy(device_io_name, mergedname.size() + 1); @@ -257,7 +256,7 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) LOG(WARNING) << "Cannot open deviceio" << device_io_name; std::cout << "Cannot open deviceio" << device_io_name << std::endl; } - d_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + d_map_base = reinterpret_cast(mmap(nullptr, page_size, PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0)); if (d_map_base == reinterpret_cast(-1)) @@ -268,7 +267,7 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) } // sanity check: check test register - uint32_t writeval = TEST_REGISTER_TRACK_WRITEVAL; + uint32_t writeval = test_register_track_writeval; uint32_t readval; readval = Fpga_Multicorrelator_8sc::fpga_acquisition_test_register(writeval); if (writeval != readval) @@ -288,9 +287,9 @@ uint32_t Fpga_Multicorrelator_8sc::fpga_acquisition_test_register( { uint32_t readval = 0; // write value to test register - d_map_base[TEST_REG_ADDR] = writeval; + d_map_base[test_reg_addr] = writeval; // read value from test register - readval = d_map_base[TEST_REG_ADDR]; + readval = d_map_base[test_reg_addr]; // return read value return readval; } @@ -300,21 +299,21 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR { uint32_t k; - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter; for (k = 0; k < d_code_length_samples; k++) { - d_map_base[PROG_MEMS_ADDR] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k]; + d_map_base[prog_mems_addr] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k]; } if (d_track_pilot) { - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter; for (k = 0; k < d_code_length_samples; k++) { - d_map_base[PROG_MEMS_ADDR] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k]; + d_map_base[prog_mems_addr] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k]; } } - d_map_base[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_samples)-1; // number of samples - 1 + d_map_base[code_length_minus_1_reg_addr] = (d_code_length_samples)-1; // number of samples - 1 } @@ -340,7 +339,7 @@ void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers } - d_initial_interp_counter[i] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); + d_initial_interp_counter[i] = static_cast(floor(max_code_resampler_counter * frac_part)); } if (d_track_pilot) { @@ -357,7 +356,7 @@ void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) { frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers } - d_initial_interp_counter[d_n_correlators] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); + d_initial_interp_counter[d_n_correlators] = static_cast(floor(max_code_resampler_counter * frac_part)); } } @@ -366,13 +365,13 @@ void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void) { for (uint32_t i = 0; i < d_n_correlators; i++) { - d_map_base[INITIAL_INDEX_REG_BASE_ADDR + i] = d_initial_index[i]; - d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + i] = d_initial_interp_counter[i]; + d_map_base[initial_index_reg_base_addr + i] = d_initial_index[i]; + d_map_base[initial_interp_counter_reg_base_addr + i] = d_initial_interp_counter[i]; } if (d_track_pilot) { - d_map_base[INITIAL_INDEX_REG_BASE_ADDR + d_n_correlators] = d_initial_index[d_n_correlators]; - d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + d_n_correlators] = d_initial_interp_counter[d_n_correlators]; + d_map_base[initial_index_reg_base_addr + d_n_correlators] = d_initial_index[d_n_correlators]; + d_map_base[initial_interp_counter_reg_base_addr + d_n_correlators] = d_initial_interp_counter[d_n_correlators]; } } @@ -381,8 +380,8 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) { float d_rem_carrier_phase_in_rad_temp; - d_code_phase_step_chips_num = static_cast(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips)); - d_code_phase_rate_step_chips_num = static_cast(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_rate_step_chips)); + d_code_phase_step_chips_num = static_cast(roundf(max_code_resampler_counter * d_code_phase_step_chips)); + d_code_phase_rate_step_chips_num = static_cast(roundf(max_code_resampler_counter * d_code_phase_rate_step_chips)); if (d_rem_carrier_phase_in_rad > M_PI) { @@ -397,25 +396,25 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad; } - d_rem_carr_phase_rad_int = static_cast(roundf((d_rem_carrier_phase_in_rad_temp)*PHASE_CARR_MAX_div_PI)); - d_phase_step_rad_int = static_cast(roundf((d_phase_step_rad)*PHASE_CARR_MAX_div_PI)); // the FPGA accepts a range for the phase step between -pi and +pi - d_carrier_phase_rate_step_rad_int = static_cast(roundf((d_carrier_phase_rate_step_rad)*PHASE_CARR_MAX_div_PI)); + d_rem_carr_phase_rad_int = static_cast(roundf((d_rem_carrier_phase_in_rad_temp)*PHASE_CARR_MAX_DIV_PI)); + d_phase_step_rad_int = static_cast(roundf((d_phase_step_rad)*PHASE_CARR_MAX_DIV_PI)); // the FPGA accepts a range for the phase step between -pi and +pi + d_carrier_phase_rate_step_rad_int = static_cast(roundf((d_carrier_phase_rate_step_rad)*PHASE_CARR_MAX_DIV_PI)); } void Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(void) { - d_map_base[CODE_PHASE_STEP_CHIPS_NUM_REG_ADDR] = d_code_phase_step_chips_num; // code phase step + d_map_base[code_phase_step_chips_num_reg_addr] = d_code_phase_step_chips_num; // code phase step - d_map_base[CODE_PHASE_STEP_CHIPS_RATE] = d_code_phase_rate_step_chips_num; // code phase step rate + d_map_base[code_phase_step_chips_rate_reg_addr] = d_code_phase_rate_step_chips_num; // code phase step rate - d_map_base[NSAMPLES_MINUS_1_REG_ADDR] = d_correlator_length_samples - 1; // number of samples + d_map_base[nsamples_minus_1_reg_addr] = d_correlator_length_samples - 1; // number of samples - d_map_base[REM_CARR_PHASE_RAD_REG_ADDR] = d_rem_carr_phase_rad_int; // initial nco phase + d_map_base[rem_carr_phase_rad_reg_addr] = d_rem_carr_phase_rad_int; // initial nco phase - d_map_base[PHASE_STEP_RAD_REG_ADDR] = d_phase_step_rad_int; // nco phase step + d_map_base[phase_step_rad_reg_addr] = d_phase_step_rad_int; // nco phase step - d_map_base[PHASE_STEP_RATE_REG_ADDR] = d_carrier_phase_rate_step_rad_int; // nco phase step rate + d_map_base[phase_step_rate_reg_addr] = d_carrier_phase_rate_step_rad_int; // nco phase step rate } @@ -429,7 +428,7 @@ void Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga(void) std::cerr << "Error launching the FPGA multicorrelator" << std::endl; } // writing 1 to reg 14 launches the tracking - d_map_base[START_FLAG_ADDR] = 1; + d_map_base[start_flag_addr] = 1; } @@ -440,14 +439,14 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) for (uint32_t k = 0; k < d_n_correlators; k++) { - readval_real = d_map_base[RESULT_REG_REAL_BASE_ADDR + k]; - readval_imag = d_map_base[RESULT_REG_IMAG_BASE_ADDR + k]; + readval_real = d_map_base[result_reg_real_base_addr + k]; + readval_imag = d_map_base[result_reg_imag_base_addr + k]; d_corr_out[k] = gr_complex(readval_real, readval_imag); } if (d_track_pilot) { - readval_real = d_map_base[RESULT_REG_REAL_BASE_ADDR + d_n_correlators]; - readval_imag = d_map_base[RESULT_REG_IMAG_BASE_ADDR + d_n_correlators]; + readval_real = d_map_base[result_reg_real_base_addr + d_n_correlators]; + readval_imag = d_map_base[result_reg_imag_base_addr + d_n_correlators]; d_Prompt_Data[0] = gr_complex(readval_real, readval_imag); } } @@ -456,15 +455,17 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) void Fpga_Multicorrelator_8sc::unlock_channel(void) { // unlock the channel to let the next samples go through - d_map_base[DROP_SAMPLES_REG_ADDR] = 1; // unlock the channel - d_map_base[STOP_TRACKING_REG_ADDR] = 1; // set the tracking module back to idle + d_map_base[drop_samples_reg_addr] = drop_samples; // unlock the channel and disable secondary codes + d_map_base[stop_tracking_reg_addr] = 1; // set the tracking module back to idle + + d_secondary_code_enabled = false; } void Fpga_Multicorrelator_8sc::close_device() { auto *aux = const_cast(d_map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + if (munmap(static_cast(aux), page_size) == -1) { std::cout << "Failed to unmap memory uio" << std::endl; } @@ -475,5 +476,97 @@ void Fpga_Multicorrelator_8sc::close_device() void Fpga_Multicorrelator_8sc::lock_channel(void) { // lock the channel for processing - d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // lock the channel + d_map_base[drop_samples_reg_addr] = 0; // lock the channel +} + + +void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length) +{ + d_secondary_code_0_length = secondary_code_0_length; + d_secondary_code_1_length = secondary_code_1_length; + + uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1; + uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1; + + d_map_base[secondary_code_lengths_reg_addr] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1; +} + +void Fpga_Multicorrelator_8sc::update_prn_code_length(uint32_t first_prn_length, uint32_t next_prn_length) +{ + d_map_base[first_prn_length_minus_1_reg_addr] = first_prn_length - 1; + d_map_base[next_prn_length_minus_1_reg_addr] = next_prn_length - 1; +} + +void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string) +{ + uint32_t secondary_code_length; + uint32_t reg_addr; + if (secondary_code == 0) + { + secondary_code_length = d_secondary_code_0_length; + reg_addr = prog_secondary_code_0_data_reg_addr; + } + else + { + secondary_code_length = d_secondary_code_1_length; + reg_addr = prog_secondary_code_1_data_reg_addr; + } + Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr); +} + + +void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr) +{ + uint32_t num_words = ceil(((float)secondary_code_length) / secondary_code_word_size); + uint32_t last_word_size = secondary_code_length % secondary_code_word_size; + + if (last_word_size == 0) + { + last_word_size = secondary_code_word_size; + } + + uint32_t write_val = 0U; + uint32_t pow_k; + uint32_t mem_addr; + if (num_words > 1) + { + for (mem_addr = 0; mem_addr < num_words - 1; mem_addr++) + { + write_val = 0U; + pow_k = 1; + for (unsigned int k = 0; k < secondary_code_word_size; k++) + { + std::string string_tmp(1, secondary_code_string->at(mem_addr * secondary_code_word_size + k)); + write_val = write_val | std::stoi(string_tmp) * pow_k; + pow_k = pow_k * 2; + } + write_val = write_val | mem_addr * secondary_code_addr_bits | secondary_code_wr_strobe; + d_map_base[reg_addr] = write_val; + } + } + write_val = 0U; + pow_k = 1; + mem_addr = num_words - 1; + + for (unsigned int k = 0; k < last_word_size; k++) + { + std::string string_tmp(1, secondary_code_string->at(mem_addr * secondary_code_word_size + k)); + write_val = write_val | std::stoi(string_tmp) * pow_k; + pow_k = pow_k * 2; + } + + write_val = write_val | (mem_addr * secondary_code_addr_bits) | (secondary_code_wr_strobe); + d_map_base[reg_addr] = write_val; +} + +void Fpga_Multicorrelator_8sc::enable_secondary_codes() +{ + d_map_base[drop_samples_reg_addr] = init_secondary_code_addresses | enable_secondary_code; // enable secondary codes and clear secondary code indices + d_secondary_code_enabled = true; +} + +void Fpga_Multicorrelator_8sc::disable_secondary_codes() +{ + // this function is to be called before starting the tracking process in order to disable the secondary codes by default + d_map_base[drop_samples_reg_addr] = drop_samples; } diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index b7ca95140..8513dec25 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -29,7 +29,7 @@ * 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 . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,33 +40,9 @@ #include #include -// FPGA register addresses - -// write addresses -#define CODE_PHASE_STEP_CHIPS_NUM_REG_ADDR 0 -#define INITIAL_INDEX_REG_BASE_ADDR 1 -#define INITIAL_INTERP_COUNTER_REG_BASE_ADDR 7 -#define NSAMPLES_MINUS_1_REG_ADDR 13 -#define CODE_LENGTH_MINUS_1_REG_ADDR 14 -#define REM_CARR_PHASE_RAD_REG_ADDR 15 -#define PHASE_STEP_RAD_REG_ADDR 16 -#define PROG_MEMS_ADDR 17 -#define DROP_SAMPLES_REG_ADDR 18 -#define INITIAL_COUNTER_VALUE_REG_ADDR_LSW 19 -#define INITIAL_COUNTER_VALUE_REG_ADDR_MSW 20 -#define CODE_PHASE_STEP_CHIPS_RATE 21 -#define PHASE_STEP_RATE_REG_ADDR 22 -#define STOP_TRACKING_REG_ADDR 23 -#define INT_ON_RST_REG_ADDR 24 // cause interrupt on reset to prevent deadlock -#define START_FLAG_ADDR 30 -// read-write addresses -#define TEST_REG_ADDR 31 -// read addresses -#define RESULT_REG_REAL_BASE_ADDR 1 -#define RESULT_REG_IMAG_BASE_ADDR 7 -#define SAMPLE_COUNTER_REG_ADDR_LSW 13 -#define SAMPLE_COUNTER_REG_ADDR_MSW 14 - +// floating point math constants related to the parameters that are written in the FPGA +#define PHASE_CARR_MAX_DIV_PI 683565275.5764316 // 2^(31)/pi +#define TWO_PI 6.283185307179586 /*! * \brief Class that implements carrier wipe-off and correlators. @@ -74,27 +50,154 @@ class Fpga_Multicorrelator_8sc { public: + /*! + * \brief Constructor + */ Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, - uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t multicorr_type, uint32_t code_samples_per_chip); + uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t code_samples_per_chip); + + /*! + * \brief Destructor + */ ~Fpga_Multicorrelator_8sc(); + + /*! + * \brief Configure pointers to the FPGA multicorrelator results + */ void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data); + + /*! + * \brief Configure the local code in the FPGA multicorrelator + */ void set_local_code_and_taps( float *shifts_chips, float *prompt_data_shift, int32_t PRN); + + /*! + * \brief Configure code phase and code rate parameters in the FPGA + */ void update_local_code(); + + /*! + * \brief Perform a multicorrelation + */ void Carrier_wipeoff_multicorrelator_resampler( float rem_carrier_phase_in_rad, float phase_step_rad, float carrier_phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int32_t signal_length_samples); + + /*! + * \brief Stop the correlation process in the FPGA and free code phase and code rate parameters + */ bool free(); + + /*! + * \brief Set channel number and open the FPGA device driver + */ void set_channel(uint32_t channel); + + /*! + * \brief Set the initial sample number where the tracking process begins + */ void set_initial_sample(uint64_t samples_offset); + + /*! + * \brief Read the sample counter in the FPGA + */ uint64_t read_sample_counter(); + + /*! + * \brief Start the tracking process in the FPGA + */ void lock_channel(void); + + /*! + * \brief finish the tracking process in the FPGA + */ void unlock_channel(void); + /*! + * \brief Set the secondary code length in the FPGA. This is only used when extended coherent integration + * is enabled in the FPGA. If tracking the pilot is enabled then secondary_code_0_length is the length of the pilot + * secondary code and secondary_code_1_length is the length of the data secondary code. If tracking the pilot is disabled + * then secondary_code_0_length is the length of the data secondary code, and secondary_code_1_length must be set to zero. + */ + void set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length); + + /*! + * \brief Initialize the secondary code in the FPGA. If tracking the pilot is enabled then the pilot secondary code is + * configured when secondary_code = 0 and the data secondary code is configured when secondary_code = 1. If tracking the + * pilot is disabled then the data secondary code is configured when secondary code = 0. + */ + void initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string); + + /*! + * \brief Set the PRN length in the FPGA in number of samples. This function is only used then extended coherent integration is enabled in the + * FPGA. The FPGA allows for the configuration of two PRN lengths. When the length of the extended coherent integration is bigger than the + * length of the PRN code, the FPGA uses the first_length_secondary_code as the length of the PRN code immediately following the beginning + * of the extended coherent integration, and the next_length_secondary_code as the length of the remaining PRN codes. + * The purpose of this is to have the option to allow the FPGA to compensate for a possible deviation between the nominal value of the PRN + * code length and the measured PRN code length in the PRN immediately following the start of the coherent integration only. + * If this option is not used then write the same value to first_length_secondary_code and next_length_secondary_code. + */ + void update_prn_code_length(uint32_t first_prn_length, uint32_t next_prn_length); + + /*! + * \brief Enable the use of secondary codes in the FPGA + */ + void enable_secondary_codes(); + + /*! + * \brief Disable the use of secondary codes in the FPGA + */ + void disable_secondary_codes(); + + private: + // FPGA register addresses + + // write addresses + static const uint32_t code_phase_step_chips_num_reg_addr = 0; + static const uint32_t initial_index_reg_base_addr = 1; + static const uint32_t initial_interp_counter_reg_base_addr = 7; + static const uint32_t nsamples_minus_1_reg_addr = 13; + static const uint32_t code_length_minus_1_reg_addr = 14; + static const uint32_t rem_carr_phase_rad_reg_addr = 15; + static const uint32_t phase_step_rad_reg_addr = 16; + static const uint32_t prog_mems_addr = 17; + static const uint32_t drop_samples_reg_addr = 18; + static const uint32_t initial_counter_value_reg_addr_lsw = 19; + static const uint32_t initial_counter_value_reg_addr_msw = 20; + static const uint32_t code_phase_step_chips_rate_reg_addr = 21; + static const uint32_t phase_step_rate_reg_addr = 22; + static const uint32_t stop_tracking_reg_addr = 23; + static const uint32_t secondary_code_lengths_reg_addr = 25; + static const uint32_t prog_secondary_code_0_data_reg_addr = 26; + static const uint32_t prog_secondary_code_1_data_reg_addr = 27; + static const uint32_t first_prn_length_minus_1_reg_addr = 28; + static const uint32_t next_prn_length_minus_1_reg_addr = 29; + static const uint32_t start_flag_addr = 30; + // read-write addresses + static const uint32_t test_reg_addr = 31; + // read addresses + static const uint32_t result_reg_real_base_addr = 1; + static const uint32_t result_reg_imag_base_addr = 7; + static const uint32_t sample_counter_reg_addr_lsw = 13; + static const uint32_t sample_counter_reg_addr_msw = 14; + // FPGA-related constants + static const uint32_t secondary_code_word_size = 20; // the secondary codes are written in to the FPGA in words of secondary_code_word_size bits + static const uint32_t secondary_code_wr_strobe = 0x800000; // write strobe position in the secondary code write register + static const uint32_t secondary_code_addr_bits = 0x100000; // memory address position in the secondary code write register + static const uint32_t drop_samples = 1; // bit 0 of drop_samples_reg_addr + static const uint32_t enable_secondary_code = 2; // bit 1 of drop_samples_reg_addr + static const uint32_t init_secondary_code_addresses = 4; // bit 2 of drop_samples_reg_addr + static const uint32_t page_size = 0x10000; + static const uint32_t max_length_deviceio_name = 50; + static const uint32_t max_code_resampler_counter = 1 << 20; // 2^(number of bits of precision of the code resampler) + static const uint32_t local_code_fpga_clear_address_counter = 0x10000000; + static const uint32_t test_register_track_writeval = 0x55AA; + gr_complex *d_corr_out; gr_complex *d_Prompt_Data; float *d_shifts_chips; @@ -116,6 +219,8 @@ private: float d_rem_carrier_phase_in_rad; float d_phase_step_rad; float d_carrier_phase_rate_step_rad; + uint32_t d_code_samples_per_chip; + bool d_track_pilot; // configuration data computed in the format that the FPGA expects uint32_t *d_initial_index; @@ -131,13 +236,14 @@ private: std::string d_device_name; uint32_t d_device_base; + // PRN codes int32_t *d_ca_codes; int32_t *d_data_codes; - uint32_t d_code_samples_per_chip; - bool d_track_pilot; - - uint32_t d_multicorr_type; + // secondary code configuration + uint32_t d_secondary_code_0_length; + uint32_t d_secondary_code_1_length; + bool d_secondary_code_enabled; // private functions uint32_t fpga_acquisition_test_register(uint32_t writeval); @@ -149,6 +255,7 @@ private: void fpga_launch_multicorrelator_fpga(void); void read_tracking_gps_results(void); void close_device(void); + void write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr); }; #endif /* GNSS_SDR_FPGA_MULTICORRELATOR_H_ */ diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.cc b/src/core/libs/gnss_sdr_fpga_sample_counter.cc index 85c6f6018..b20132a50 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.cc +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.cc @@ -43,8 +43,6 @@ #include // for write, close, read, ssize_t -#define PAGE_SIZE 0x10000 // default page size for the multicorrelator memory map -#define TEST_REG_SANITY_CHECK 0x55AA // value to check the presence of the test register (to detect the hw) #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ @@ -151,7 +149,7 @@ void gnss_sdr_fpga_sample_counter::open_device() LOG(WARNING) << "Cannot open deviceio" << device_name; std::cout << "Counter-Intr: cannot open deviceio" << device_name << std::endl; } - map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + map_base = reinterpret_cast(mmap(nullptr, page_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); if (map_base == reinterpret_cast(-1)) @@ -161,7 +159,7 @@ void gnss_sdr_fpga_sample_counter::open_device() } // sanity check : check test register - uint32_t writeval = TEST_REG_SANITY_CHECK; + uint32_t writeval = test_reg_sanity_check; uint32_t readval; readval = gnss_sdr_fpga_sample_counter::test_register(writeval); if (writeval != readval) @@ -181,7 +179,7 @@ void gnss_sdr_fpga_sample_counter::close_device() map_base[2] = 0; // disable the generation of the interrupt in the device auto *aux = const_cast(map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + if (munmap(static_cast(aux), page_size) == -1) { std::cout << "Failed to unmap memory uio" << std::endl; } diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.h b/src/core/libs/gnss_sdr_fpga_sample_counter.h index 61d4a5864..4207be1e1 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.h +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.h @@ -55,6 +55,9 @@ public: gr_vector_void_star &output_items); private: + static const uint32_t page_size = 0x10000; // default page size for the multicorrelator memory map + static const uint32_t test_reg_sanity_check = 0x55AA; // value to check the presence of the test register (to detect the hw) + friend gnss_sdr_fpga_sample_counter_sptr gnss_sdr_make_fpga_sample_counter(double _fs, int32_t _interval_ms); gnss_sdr_fpga_sample_counter(double _fs, int32_t _interval_ms); uint32_t test_register(uint32_t writeval);