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 898ba8d62..bdfc0bed7 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,6 +44,13 @@ #include // for complex #include // for memcpy +// 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, @@ -67,7 +74,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( acq_parameters.repeat_satellite = configuration_->property(role + ".repeat_satellite", false); DLOG(INFO) << role << " satellite repeat = " << acq_parameters.repeat_satellite; - float downsampling_factor = configuration_->property(role + ".downsampling_factor", 4.0); + uint32_t downsampling_factor = configuration_->property(role + ".downsampling_factor", 4); acq_parameters.downsampling_factor = downsampling_factor; fs_in = fs_in / downsampling_factor; @@ -97,15 +104,16 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( acq_parameters.device_name = device_name; acq_parameters.samples_per_ms = nsamples_total / sampled_ms; acq_parameters.samples_per_code = nsamples_total; - acq_parameters.excludelimit = static_cast(std::round(static_cast(fs_in) / GALILEO_E1_CODE_CHIP_RATE_HZ)); + acq_parameters.excludelimit = static_cast(1 + ceil((1.0 / GALILEO_E1_CODE_CHIP_RATE_HZ) * static_cast(fs_in))); // compute all the GALILEO E1 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT auto* code = new std::complex[nsamples_total]; // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new lv_16sc_t[nsamples_total * GALILEO_E1_NUMBER_OF_CODES]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + d_all_fft_codes_ = new uint32_t[(nsamples_total * GALILEO_E1_NUMBER_OF_CODES)]; // memory containing all the possible fft codes for PRN 0 to 32 + float max; // temporary maxima search + int32_t tmp, tmp2, local_code, fft_data; for (uint32_t PRN = 1; PRN <= GALILEO_E1_NUMBER_OF_CODES; PRN++) { @@ -153,17 +161,26 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( max = std::abs(fft_codes_padded[i].imag()); } } - for (uint32_t i = 0; i < nsamples_total; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // and package codes in a format that is ready to be written to the FPGA + for (uint32_t i = 0; i < nsamples_total; i++) { - d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, 9) - 1) / max)), - static_cast(floor(fft_codes_padded[i].imag() * (pow(2, 9) - 1) / max))); + 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; + d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } acq_parameters.all_fft_codes = d_all_fft_codes_; + acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters.max_num_acqs = configuration_->property(role + ".max_num_acqs", 2); // reference for the FPGA FFT-IFFT attenuation factor - acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14); + acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 12); acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); 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 8683d5556..ff63d49c9 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 @@ -166,7 +166,7 @@ private: unsigned int in_streams_; unsigned int out_streams_; - lv_16sc_t* d_all_fft_codes_; // memory that contains all the code ffts + uint32_t* d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ */ 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 0b8e0c815..ed47a80ce 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -44,6 +44,13 @@ #include // for complex #include // for strcpy, memcpy +// 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, @@ -64,7 +71,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf acq_parameters.repeat_satellite = configuration_->property(role + ".repeat_satellite", false); DLOG(INFO) << role << " satellite repeat = " << acq_parameters.repeat_satellite; - float downsampling_factor = configuration_->property(role + ".downsampling_factor", 1.0); + uint32_t downsampling_factor = configuration_->property(role + ".downsampling_factor", 1); acq_parameters.downsampling_factor = downsampling_factor; fs_in = fs_in / downsampling_factor; @@ -98,15 +105,17 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf acq_parameters.samples_per_ms = nsamples_total / sampled_ms; acq_parameters.samples_per_code = nsamples_total; - acq_parameters.excludelimit = static_cast(ceil((1.0 / GALILEO_E5A_CODE_CHIP_RATE_HZ) * static_cast(acq_parameters.fs_in))); + acq_parameters.excludelimit = static_cast(1 + ceil((1.0 / GALILEO_E5A_CODE_CHIP_RATE_HZ) * static_cast(fs_in))); // compute all the GALILEO E5 PRN Codes (this is done only once in the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT auto* code = new std::complex[nsamples_total]; // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new lv_16sc_t[nsamples_total * GALILEO_E5A_NUMBER_OF_CODES]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + d_all_fft_codes_ = new uint32_t[(nsamples_total * GALILEO_E5A_NUMBER_OF_CODES)]; // memory containing all the possible fft codes for PRN 0 to 32 + + float max; // temporary maxima search + int32_t tmp, tmp2, local_code, fft_data; for (uint32_t PRN = 1; PRN <= GALILEO_E5A_NUMBER_OF_CODES; PRN++) { @@ -154,18 +163,27 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf max = std::abs(fft_codes_padded[i].imag()); } } - for (uint32_t i = 0; i < nsamples_total; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // and package codes in a format that is ready to be written to the FPGA + for (uint32_t i = 0; i < nsamples_total; i++) { - d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, 9) - 1) / max)), - static_cast(floor(fft_codes_padded[i].imag() * (pow(2, 9) - 1) / max))); + 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; + d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } acq_parameters.all_fft_codes = d_all_fft_codes_; // reference for the FPGA FFT-IFFT attenuation factor - acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14); + acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 13); + acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters.max_num_acqs = configuration_->property(role + ".max_num_acqs", 2); acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); channel_ = 0; 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 62c186503..8863355bb 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -184,7 +184,7 @@ private: Gnss_Synchro* gnss_synchro_; - lv_16sc_t* d_all_fft_codes_; // memory that contains all the code ffts + uint32_t* d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GALILEO_E5A_PCPS_ACQUISITION_FPGA_H_ */ 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 ff300ea57..9cb95ba34 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,9 +47,15 @@ #include // for complex #include // for memcpy - #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, @@ -71,9 +77,8 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( acq_parameters.repeat_satellite = configuration_->property(role + ".repeat_satellite", false); DLOG(INFO) << role << " satellite repeat = " << acq_parameters.repeat_satellite; - float downsampling_factor = configuration_->property(role + ".downsampling_factor", 4.0); + uint32_t downsampling_factor = configuration_->property(role + ".downsampling_factor", 4); acq_parameters.downsampling_factor = downsampling_factor; - fs_in = fs_in / downsampling_factor; acq_parameters.fs_in = fs_in; @@ -94,7 +99,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( acq_parameters.device_name = device_name; acq_parameters.samples_per_ms = nsamples_total / sampled_ms; acq_parameters.samples_per_code = nsamples_total; - acq_parameters.excludelimit = static_cast(std::round(static_cast(fs_in) / GPS_L1_CA_CODE_RATE_HZ)); + acq_parameters.excludelimit = static_cast(1 + ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(fs_in))); // compute all the GPS L1 PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) @@ -102,8 +107,10 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // allocate memory to compute all the PRNs and compute all the possible codes auto* code = new std::complex[nsamples_total]; // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new lv_16sc_t[nsamples_total * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 + float max; + int32_t tmp, tmp2, local_code, fft_data; + // temporary maxima search for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l1_ca_code_gen_complex_sampled(code, PRN, fs_in, 0); // generate PRN code @@ -135,10 +142,15 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( max = std::abs(fft_codes_padded[i].imag()); } } - for (uint32_t i = 0; i < nsamples_total; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // and package codes in a format that is ready to be written to the FPGA + for (uint32_t i = 0; i < nsamples_total; i++) { - d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, 9) - 1) / max)), - static_cast(floor(fft_codes_padded[i].imag() * (pow(2, 9) - 1) / max))); + 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; + d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } @@ -146,8 +158,12 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( acq_parameters.all_fft_codes = d_all_fft_codes_; // reference for the FPGA FFT-IFFT attenuation factor - acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14); + acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 11); + acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters.max_num_acqs = configuration_->property(role + ".max_num_acqs", 2); acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); channel_ = 0; 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 6204354b2..2b627ec00 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 @@ -35,6 +35,7 @@ #ifndef GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FPGA_H_ #define GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FPGA_H_ + #include "channel_fsm.h" #include "pcps_acquisition_fpga.h" #include // for basic_block_sptr, top_block_sptr @@ -100,8 +101,8 @@ public: } /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::shared_ptr channel_fsm) override { channel_fsm_ = channel_fsm; @@ -166,7 +167,7 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - lv_16sc_t* d_all_fft_codes_; // memory that contains all the code ffts + uint32_t* d_all_fft_codes_; // memory that contains all the code ffts }; #endif /* GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FPGA_H_ */ 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 6dcb7e224..2e289b440 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 @@ -47,7 +47,11 @@ #include // for memcpy #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, @@ -102,8 +106,11 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( // allocate memory to compute all the PRNs and compute all the possible codes auto* code = new std::complex[nsamples_total]; // buffer for the local code auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new lv_16sc_t[nsamples_total * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32 - float max; // temporary maxima search + //d_all_fft_codes_ = new lv_16sc_t[nsamples_total * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32 + d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 + float max; // temporary maxima search + int32_t tmp, tmp2, local_code, fft_data; + for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l2c_m_code_gen_complex_sampled(code, PRN, fs_in_); @@ -127,10 +134,18 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( max = std::abs(fft_codes_padded[i].imag()); } } - for (unsigned int i = 0; i < nsamples_total; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // and package codes in a format that is ready to be written to the FPGA + for (uint32_t i = 0; i < nsamples_total; i++) { - d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, 7) - 1) / max)), - static_cast(floor(fft_codes_padded[i].imag() * (pow(2, 7) - 1) / max))); + 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; + d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; + + // d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)), + // static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max))); } } 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 495a613e5..e99b40508 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 @@ -169,7 +169,7 @@ private: unsigned int in_streams_; unsigned int out_streams_; - lv_16sc_t* d_all_fft_codes_; // memory that contains all the code ffts + uint32_t* d_all_fft_codes_; // memory that contains all the code ffts //float calculate_threshold(float pfa); }; 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 413f68783..ceeaed722 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -49,6 +49,14 @@ #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, @@ -70,7 +78,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( acq_parameters.repeat_satellite = configuration_->property(role + ".repeat_satellite", false); DLOG(INFO) << role << " satellite repeat = " << acq_parameters.repeat_satellite; - float downsampling_factor = configuration_->property(role + ".downsampling_factor", 1.0); + uint32_t downsampling_factor = configuration_->property(role + ".downsampling_factor", 1); acq_parameters.downsampling_factor = downsampling_factor; fs_in = fs_in / downsampling_factor; @@ -96,16 +104,18 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( acq_parameters.samples_per_ms = nsamples_total / sampled_ms; acq_parameters.samples_per_code = nsamples_total; - acq_parameters.excludelimit = static_cast(ceil((1.0 / GPS_L5I_CODE_RATE_HZ) * static_cast(acq_parameters.fs_in))); + acq_parameters.excludelimit = static_cast(1 + ceil((1.0 / GPS_L5I_CODE_RATE_HZ) * static_cast(fs_in))); // compute all the GPS L5 PRN Codes (this is done only once upon the class constructor in order to avoid re-computing the PRN codes every time // a channel is assigned) auto* fft_if = new gr::fft::fft_complex(nsamples_total, true); // Direct FFT auto* code = new gr_complex[nsamples_total]; auto* fft_codes_padded = static_cast(volk_gnsssdr_malloc(nsamples_total * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_all_fft_codes_ = new lv_16sc_t[nsamples_total * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32 + d_all_fft_codes_ = new uint32_t[(nsamples_total * NUM_PRNs)]; // memory containing all the possible fft codes for PRN 0 to 32 float max; // temporary maxima search + int32_t tmp, tmp2, local_code, fft_data; + for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l5i_code_gen_complex_sampled(code, PRN, fs_in); @@ -136,18 +146,27 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( max = std::abs(fft_codes_padded[i].imag()); } } - for (uint32_t i = 0; i < nsamples_total; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs + // and package codes in a format that is ready to be written to the FPGA + for (uint32_t i = 0; i < nsamples_total; i++) { - d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast(floor(fft_codes_padded[i].real() * (pow(2, 9) - 1) / max)), - static_cast(floor(fft_codes_padded[i].imag() * (pow(2, 9) - 1) / max))); + 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; + d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } acq_parameters.all_fft_codes = d_all_fft_codes_; // reference for the FPGA FFT-IFFT attenuation factor - acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14); + acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 11); + acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters.max_num_acqs = configuration_->property(role + ".max_num_acqs", 2); acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters); channel_ = 0; 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 caecc68a8..cf1c9e856 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -106,6 +106,7 @@ public: channel_fsm_ = channel_fsm; acquisition_fpga_->set_channel_fsm(channel_fsm); } + /*! * \brief Set statistics threshold of PCPS algorithm */ @@ -167,7 +168,7 @@ private: unsigned int in_streams_; unsigned int out_streams_; - lv_16sc_t* d_all_fft_codes_; // memory that contains all the code ffts + uint32_t* d_all_fft_codes_; // memory that contains all the code ffts float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index e3e5fdd60..2ac9bc671 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -34,6 +34,7 @@ #include "pcps_acquisition_fpga.h" #include "gnss_synchro.h" +//#include #include #include // for ceil #include // for operator<< @@ -42,7 +43,6 @@ #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_))); @@ -52,7 +52,7 @@ pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_) pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) { acq_parameters = std::move(conf_); - d_sample_counter = 0ULL; // SAMPLE COUNTER + d_sample_counter = 0ULL; // Sample Counter d_active = false; d_state = 0; d_fft_size = acq_parameters.samples_per_code; @@ -71,6 +71,15 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) d_total_block_exp = acq_parameters.total_block_exp; + d_make_2_steps = acq_parameters.make_2_steps; + d_num_doppler_bins_step2 = acq_parameters.num_doppler_bins_step2; + d_doppler_step2 = acq_parameters.doppler_step2; + d_doppler_center_step_two = 0.0; + + d_doppler_max = acq_parameters.doppler_max; + + d_max_num_acqs = acq_parameters.max_num_acqs; + acquisition_fpga = std::make_shared(acq_parameters.device_name, acq_parameters.code_length, acq_parameters.doppler_max, d_fft_size, acq_parameters.fs_in, acq_parameters.sampled_ms, acq_parameters.select_queue_Fpga, acq_parameters.all_fft_codes, acq_parameters.excludelimit); } @@ -100,9 +109,7 @@ void pcps_acquisition_fpga::init() d_mag = 0.0; d_input_power = 0.0; - d_num_doppler_bins = static_cast(std::ceil(static_cast(static_cast(acq_parameters.doppler_max) - static_cast(-acq_parameters.doppler_max)) / static_cast(d_doppler_step))) + 1; - - acquisition_fpga->init(); + d_num_doppler_bins = static_cast(std::ceil(static_cast(static_cast(d_doppler_max) - static_cast(-d_doppler_max)) / static_cast(d_doppler_step))) + 1; } @@ -171,57 +178,52 @@ void pcps_acquisition_fpga::send_negative_acquisition() } } - -void pcps_acquisition_fpga::set_active(bool active) +void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_min) { - d_active = active; - - // initialize acquisition algorithm uint32_t indext = 0U; float firstpeak = 0.0; float secondpeak = 0.0; uint32_t total_block_exp; - - d_input_power = 0.0; - d_mag = 0.0; - - int32_t doppler; - - DLOG(INFO) << "Channel: " << d_channel - << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << " ,sample stamp: " << d_sample_counter << ", threshold: " - << d_threshold << ", doppler_max: " << acq_parameters.doppler_max - << ", doppler_step: " << d_doppler_step - // no CFAR algorithm in the FPGA - << ", use_CFAR_algorithm_flag: false"; - uint64_t initial_sample; - - acquisition_fpga->configure_acquisition(); - acquisition_fpga->set_doppler_sweep(d_num_doppler_bins); - acquisition_fpga->write_local_code(); - acquisition_fpga->set_block_exp(d_total_block_exp); + int32_t doppler; + acquisition_fpga->set_doppler_sweep(num_doppler_bins, doppler_step, doppler_min); acquisition_fpga->run_acquisition(); - acquisition_fpga->read_acquisition_results(&indext, &firstpeak, &secondpeak, &initial_sample, &d_input_power, &d_doppler_index, &total_block_exp); + acquisition_fpga->read_acquisition_results(&indext, + &firstpeak, + &secondpeak, + &initial_sample, + &d_input_power, + &d_doppler_index, + &total_block_exp); + + doppler = static_cast(doppler_min) + doppler_step * (d_doppler_index - 1); if (total_block_exp > d_total_block_exp) { // if the attenuation factor of the FPGA FFT-IFFT is smaller than the reference attenuation factor then we need to update the reference attenuation factor std::cout << "changing blk exp..... d_total_block_exp = " << d_total_block_exp << " total_block_exp = " << total_block_exp << " chan = " << d_channel << std::endl; d_total_block_exp = total_block_exp; - } - - doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * (d_doppler_index - 1); - - if (secondpeak > 0) - { - d_test_statistics = firstpeak / secondpeak; + d_test_statistics = 0; } else { - d_test_statistics = 0.0; + if (secondpeak > 0) + { + d_test_statistics = firstpeak / secondpeak; + } + else + { + d_test_statistics = 0.0; + } } + // 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; @@ -230,7 +232,7 @@ void pcps_acquisition_fpga::set_active(bool active) if (d_downsampling_factor > 1) { d_gnss_synchro->Acq_delay_samples = static_cast(d_downsampling_factor * (indext)); - d_gnss_synchro->Acq_samplestamp_samples = d_downsampling_factor * d_sample_counter - 44; //33; //41; //+ 81*0.5; // delay due to the downsampling filter in the acquisition + d_gnss_synchro->Acq_samplestamp_samples = d_downsampling_factor * static_cast(d_sample_counter) - static_cast(44); //33; //41; //+ 81*0.5; // delay due to the downsampling filter in the acquisition } else { @@ -243,18 +245,81 @@ void pcps_acquisition_fpga::set_active(bool active) d_gnss_synchro->Acq_delay_samples = static_cast(indext); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; // delay due to the downsampling filter in the acquisition } +} - if (d_test_statistics > d_threshold) + +void pcps_acquisition_fpga::set_active(bool active) +{ + d_active = active; + + d_input_power = 0.0; + + d_mag = 0.0; + + DLOG(INFO) << "Channel: " << d_channel + << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << " ,sample stamp: " << d_sample_counter << ", threshold: " + << d_threshold << ", doppler_max: " << d_doppler_max + << ", doppler_step: " << d_doppler_step + // no CFAR algorithm in the FPGA + << ", use_CFAR_algorithm_flag: false"; + + acquisition_fpga->open_device(); + acquisition_fpga->configure_acquisition(); + 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); + if (!d_make_2_steps) { - d_active = false; - send_positive_acquisition(); - d_state = 0; // Positive acquisition + acquisition_fpga->close_device(); + if (d_test_statistics > d_threshold) + { + d_active = false; + send_positive_acquisition(); + d_state = 0; // Positive acquisition + } + else + { + d_state = 0; + d_active = false; + send_negative_acquisition(); + } } else { - d_state = 0; - d_active = false; - send_negative_acquisition(); + if (d_test_statistics > d_threshold) + { + d_doppler_center_step_two = static_cast(d_gnss_synchro->Acq_doppler_hz); + + uint32_t num_second_acq = 1; + + 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); + if (d_test_statistics > d_threshold) + { + d_active = false; + send_positive_acquisition(); + d_state = 0; // Positive acquisition + break; + } + num_second_acq = num_second_acq + 1; + } + if (d_test_statistics <= d_threshold) + { + d_state = 0; + d_active = false; + send_negative_acquisition(); + } + } + else + { + acquisition_fpga->close_device(); + d_state = 0; + d_active = false; + send_negative_acquisition(); + } } } @@ -262,11 +327,7 @@ void pcps_acquisition_fpga::set_active(bool active) void pcps_acquisition_fpga::reset_acquisition(void) { // this function triggers a HW reset of the FPGA PL. + acquisition_fpga->open_device(); acquisition_fpga->reset_acquisition(); -} - - -void pcps_acquisition_fpga::read_fpga_total_scale_factor(uint32_t* total_scale_factor, uint32_t* fw_scale_factor) -{ - acquisition_fpga->read_fpga_total_scale_factor(total_scale_factor, fw_scale_factor); + acquisition_fpga->close_device(); } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index 174ee7af5..cf2ff1111 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -62,11 +62,16 @@ typedef struct int32_t code_length; uint32_t select_queue_Fpga; std::string device_name; - lv_16sc_t* all_fft_codes; // memory that contains all the code ffts - float downsampling_factor; + uint32_t* all_fft_codes; // pointer to memory that contains all the code ffts + //float downsampling_factor; + uint32_t downsampling_factor; uint32_t total_block_exp; uint32_t excludelimit; + bool make_2_steps; + uint32_t num_doppler_bins_step2; + float doppler_step2; bool repeat_satellite; + uint32_t max_num_acqs; } pcpsconf_fpga_t; class pcps_acquisition_fpga; @@ -95,6 +100,8 @@ private: 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); + void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_max); + pcpsconf_fpga_t acq_parameters; bool d_active; float d_threshold; @@ -106,17 +113,25 @@ private: uint32_t d_channel; std::shared_ptr d_channel_fsm; uint32_t d_doppler_step; + uint32_t d_doppler_max; uint32_t d_fft_size; uint32_t d_num_doppler_bins; uint64_t d_sample_counter; Gnss_Synchro* d_gnss_synchro; std::shared_ptr acquisition_fpga; - float d_downsampling_factor; + //float d_downsampling_factor; + uint32_t d_downsampling_factor; uint32_t d_select_queue_Fpga; uint32_t d_total_block_exp; + bool d_make_2_steps; + uint32_t d_num_doppler_bins_step2; + float d_doppler_step2; + float d_doppler_center_step_two; + uint32_t d_max_num_acqs; + public: ~pcps_acquisition_fpga(); @@ -172,10 +187,9 @@ public: d_channel = channel; } - /*! - * \brief Set channel fsm associated to this acquisition instance - */ + * \brief Set channel fsm associated to this acquisition instance + */ inline void set_channel_fsm(std::shared_ptr channel_fsm) { d_channel_fsm = channel_fsm; @@ -197,7 +211,7 @@ public: */ inline void set_doppler_max(uint32_t doppler_max) { - acq_parameters.doppler_max = doppler_max; + d_doppler_max = doppler_max; acquisition_fpga->set_doppler_max(doppler_max); } @@ -215,11 +229,6 @@ public: * \brief This funciton triggers a HW reset of the FPGA PL. */ void reset_acquisition(void); - - /*! - * \brief This funciton is only used for the unit tests - */ - void read_fpga_total_scale_factor(uint32_t* total_scale_factor, uint32_t* fw_scale_factor); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_FPGA_H_*/ diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index d42c27833..29c8c0537 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -45,23 +45,20 @@ // FPGA register parameters -#define PAGE_SIZE 0x10000 // default page size for the multicorrelator memory map -#define MAX_PHASE_STEP_RAD 0.999999999534339 // 1 - pow(2,-31); -#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_29 536870912 // 2^29 (used for the conversion of floating point numbers to integers) -#define SELECT_LSB 0x00FF // value to select the least significant byte -#define SELECT_MSB 0XFF00 // value to select the most significant byte -#define SELECT_16_BITS 0xFFFF // value to select 16 bits -#define SHL_8_BITS 256 // value used to shift a value 8 bits to the left -#define SELECT_LSBits 0x000003FF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0x000FFC00 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0x000FFFFF // Select a 20 bit word -#define SHL_CODE_BITS 1024 // shift left by 10 bits +#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) \ @@ -84,7 +81,8 @@ Fpga_Acquisition::Fpga_Acquisition(std::string device_name, int64_t fs_in, uint32_t sampled_ms __attribute__((unused)), uint32_t select_queue, - lv_16sc_t *all_fft_codes, + //lv_16sc_t *all_fft_codes, + uint32_t *all_fft_codes, uint32_t excludelimit) { uint32_t vector_length = nsamples_total; @@ -102,12 +100,10 @@ Fpga_Acquisition::Fpga_Acquisition(std::string device_name, d_fd = 0; // driver descriptor d_map_base = nullptr; // driver memory map d_all_fft_codes = all_fft_codes; - - Fpga_Acquisition::reset_acquisition(); Fpga_Acquisition::open_device(); + Fpga_Acquisition::reset_acquisition(); Fpga_Acquisition::fpga_acquisition_test_register(); Fpga_Acquisition::close_device(); - d_PRN = 0; DLOG(INFO) << "Acquisition FPGA class created"; } @@ -116,12 +112,6 @@ Fpga_Acquisition::Fpga_Acquisition(std::string device_name, Fpga_Acquisition::~Fpga_Acquisition() = default; -bool Fpga_Acquisition::init() -{ - return true; -} - - bool Fpga_Acquisition::set_local_code(uint32_t PRN) { // select the code with the chosen PRN @@ -132,8 +122,12 @@ bool Fpga_Acquisition::set_local_code(uint32_t PRN) void Fpga_Acquisition::write_local_code() { - Fpga_Acquisition::fpga_configure_acquisition_local_code( - &d_all_fft_codes[d_nsamples_total * (d_PRN - 1)]); + d_map_base[9] = LOCAL_CODE_CLEAR_MEM; + // write local code + for (uint32_t k = 0; k < d_vector_length; k++) + { + d_map_base[6] = d_all_fft_codes[d_nsamples_total * (d_PRN - 1) + k]; + } } @@ -184,26 +178,6 @@ void Fpga_Acquisition::fpga_acquisition_test_register() } -void Fpga_Acquisition::fpga_configure_acquisition_local_code(lv_16sc_t fft_local_code[]) -{ - uint32_t local_code; - uint32_t k, tmp, tmp2; - uint32_t fft_data; - - d_map_base[9] = LOCAL_CODE_CLEAR_MEM; - // write local code - for (k = 0; k < d_vector_length; k++) - { - tmp = fft_local_code[k].real(); - tmp2 = fft_local_code[k].imag(); - - 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; - d_map_base[6] = fft_data; - } -} - - void Fpga_Acquisition::run_acquisition(void) { // enable interrupts @@ -241,47 +215,30 @@ void Fpga_Acquisition::set_block_exp(uint32_t total_block_exp) d_map_base[11] = total_block_exp; } - -void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps) +void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min) { float phase_step_rad_real; - float phase_step_rad_int_temp; int32_t phase_step_rad_int; - auto doppler = static_cast(-d_doppler_max); - float phase_step_rad = GPS_TWO_PI * (doppler) / static_cast(d_fs_in); + // The doppler step can never be outside the range -pi to +pi, otherwise there would be aliasing // The FPGA expects phase_step_rad between -1 (-pi) to +1 (+pi) - // The FPGA also expects the phase to be negative since it produces cos(x) -j*sin(x) - phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2); - - // avoid saturation of the fixed point representation in the fpga - // (only the positive value can saturate due to the 2's complement representation) - if (phase_step_rad_real >= 1.0) - { - phase_step_rad_real = MAX_PHASE_STEP_RAD; - } - phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2 - phase_step_rad_int = static_cast(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings + phase_step_rad_real = 2.0 * (doppler_min) / static_cast(d_fs_in); + phase_step_rad_int = static_cast(phase_step_rad_real * (POW_2_31)); d_map_base[3] = phase_step_rad_int; // repeat the calculation with the doppler step - doppler = static_cast(d_doppler_step); - phase_step_rad = GPS_TWO_PI * (doppler) / static_cast(d_fs_in); - phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2); - if (phase_step_rad_real >= 1.0) - { - phase_step_rad_real = MAX_PHASE_STEP_RAD; - } - phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2 - phase_step_rad_int = static_cast(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings + phase_step_rad_real = 2.0 * (doppler_step) / static_cast(d_fs_in); + phase_step_rad_int = static_cast(phase_step_rad_real * (POW_2_31)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings d_map_base[4] = phase_step_rad_int; + + // write number of doppler sweeps d_map_base[5] = num_sweeps; } void Fpga_Acquisition::configure_acquisition() { - Fpga_Acquisition::open_device(); + //Fpga_Acquisition::open_device(); d_map_base[0] = d_select_queue; d_map_base[1] = d_vector_length; @@ -291,30 +248,6 @@ void Fpga_Acquisition::configure_acquisition() } -void Fpga_Acquisition::set_phase_step(uint32_t doppler_index) -{ - float phase_step_rad_real; - float phase_step_rad_int_temp; - int32_t phase_step_rad_int; - int32_t doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; - float phase_step_rad = GPS_TWO_PI * (doppler) / static_cast(d_fs_in); - // The doppler step can never be outside the range -pi to +pi, otherwise there would be aliasing - // The FPGA expects phase_step_rad between -1 (-pi) to +1 (+pi) - // The FPGA also expects the phase to be negative since it produces cos(x) -j*sin(x) - // while the gnss-sdr software (volk_gnsssdr_s32f_sincos_32fc) generates cos(x) + j*sin(x) - phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2); - // avoid saturation of the fixed point representation in the fpga - // (only the positive value can saturate due to the 2's complement representation) - if (phase_step_rad_real >= 1.0) - { - phase_step_rad_real = MAX_PHASE_STEP_RAD; - } - phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2 - phase_step_rad_int = static_cast(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings - d_map_base[3] = phase_step_rad_int; -} - - void Fpga_Acquisition::read_acquisition_results(uint32_t *max_index, float *firstpeak, float *secondpeak, uint64_t *initial_sample, float *power_sum, uint32_t *doppler_index, uint32_t *total_blk_exp) { @@ -349,9 +282,7 @@ void Fpga_Acquisition::read_acquisition_results(uint32_t *max_index, readval = d_map_base[7]; // read doppler index -- this read releases the interrupt line *doppler_index = readval; - readval = d_map_base[15]; // read dummy - - Fpga_Acquisition::close_device(); + readval = d_map_base[15]; // read dummy (to be removed) } @@ -380,9 +311,7 @@ void Fpga_Acquisition::close_device() void Fpga_Acquisition::reset_acquisition(void) { - Fpga_Acquisition::open_device(); d_map_base[8] = RESET_ACQUISITION; // writing a 2 to d_map_base[8] resets the multicorrelator - Fpga_Acquisition::close_device(); } @@ -392,8 +321,7 @@ void Fpga_Acquisition::read_fpga_total_scale_factor(uint32_t *total_scale_factor uint32_t readval = 0; readval = d_map_base[8]; *total_scale_factor = readval; - - //readval = d_map_base[8]; + // only the total scale factor is used for the tests (fw scale factor to be removed) *fw_scale_factor = 0; } diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index b64e25e4d..2ee00e315 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -53,16 +53,15 @@ public: int64_t fs_in, uint32_t sampled_ms, uint32_t select_queue, - lv_16sc_t *all_fft_codes, + uint32_t *all_fft_codes, uint32_t excludelimit); ~Fpga_Acquisition(); - bool init(); bool set_local_code(uint32_t PRN); bool free(); - void set_doppler_sweep(uint32_t num_sweeps); + void set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min); void run_acquisition(void); - void set_phase_step(uint32_t doppler_index); + void read_acquisition_results(uint32_t *max_index, float *firstpeak, float *secondpeak, uint64_t *initial_sample, float *power_sum, uint32_t *doppler_index, uint32_t *total_blk_exp); void block_samples(); @@ -110,7 +109,7 @@ private: // data related to the hardware module and the driver int32_t d_fd; // driver descriptor volatile uint32_t *d_map_base; // driver memory map - lv_16sc_t *d_all_fft_codes; // memory that contains all the code ffts + uint32_t *d_all_fft_codes; // memory that contains all the code ffts uint32_t d_vector_length; // number of samples incluing padding and number of ms uint32_t d_excludelimit; uint32_t d_nsamples_total; // number of samples including padding @@ -122,7 +121,6 @@ private: uint32_t d_PRN; // PRN // FPGA private functions void fpga_acquisition_test_register(void); - void fpga_configure_acquisition_local_code(lv_16sc_t fft_local_code[]); void read_result_valid(uint32_t *result_valid); }; diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 419df642c..d632be535 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -218,7 +218,6 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) acq_->set_local_code(); if (flag_enable_fpga) { - //set again the gnss_synchro pointer to trigger the preloading of the current PRN code to the FPGA fabric trk_->set_gnss_synchro(&gnss_synchro_); } nav_->set_satellite(gnss_signal_.get_satellite()); diff --git a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc index 0a09cc789..f83c64f25 100644 --- a/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc +++ b/src/algorithms/signal_source/adapters/ad9361_fpga_signal_source.cc @@ -53,10 +53,10 @@ Ad9361FpgaSignalSource::Ad9361FpgaSignalSource(ConfigurationInterface* configura std::string default_item_type = "gr_complex"; std::string default_dump_file = "./data/signal_source.dat"; freq_ = configuration->property(role + ".freq", GPS_L1_FREQ_HZ); - sample_rate_ = configuration->property(role + ".sampling_frequency", 2600000); - bandwidth_ = configuration->property(role + ".bandwidth", 2000000); + sample_rate_ = configuration->property(role + ".sampling_frequency", 12500000); + bandwidth_ = configuration->property(role + ".bandwidth", 12500000); rx1_en_ = configuration->property(role + ".rx1_enable", true); - rx2_en_ = configuration->property(role + ".rx2_enable", false); + rx2_en_ = configuration->property(role + ".rx2_enable", true); buffer_size_ = configuration->property(role + ".buffer_size", 0xA0000); quadrature_ = configuration->property(role + ".quadrature", true); rf_dc_ = configuration->property(role + ".rf_dc", true); 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 4d00fff10..ac916cef3 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 @@ -48,6 +48,11 @@ #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, @@ -56,8 +61,6 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( Dll_Pll_Conf_Fpga trk_param_fpga = Dll_Pll_Conf_Fpga(); DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - std::string default_item_type = "gr_complex"; - std::string item_type = configuration->property(role + ".item_type", default_item_type); 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); trk_param_fpga.fs_in = fs_in; @@ -68,16 +71,76 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.dump_filename = dump_filename; bool dump_mat = configuration->property(role + ".dump_mat", true); trk_param_fpga.dump_mat = dump_mat; + trk_param_fpga.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param_fpga.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: Gal. E1. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param_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) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } trk_param_fpga.pll_bw_hz = pll_bw_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); - if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } trk_param_fpga.dll_bw_hz = dll_bw_hz; float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); trk_param_fpga.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); trk_param_fpga.dll_bw_narrow_hz = dll_bw_narrow_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param_fpga.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param_fpga.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param_fpga.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param_fpga.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param_fpga.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param_fpga.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param_fpga.pull_in_time_s = pull_in_time_s; + int32_t extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); trk_param_fpga.early_late_space_chips = early_late_space_chips; @@ -111,16 +174,29 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( char sig_[3] = "1B"; std::memcpy(trk_param_fpga.signal, sig_, 3); int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; + 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; + 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; + 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; + if (FLAGS_carrier_lock_th != 0.85) + { + carrier_lock_th = FLAGS_carrier_lock_th; + } trk_param_fpga.carrier_lock_th = carrier_lock_th; // FPGA configuration parameters @@ -158,19 +234,39 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( galileo_e1_code_gen_sinboc11_float(ca_codes_f, pilot_signal, PRN); galileo_e1_code_gen_sinboc11_float(data_codes_f, data_signal, PRN); + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) { - d_ca_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = static_cast(ca_codes_f[s]); - d_data_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = static_cast(data_codes_f[s]); + int32_t tmp_value = static_cast(ca_codes_f[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = tmp_value; + tmp_value = static_cast(data_codes_f[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; + d_data_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = tmp_value; } } else { galileo_e1_code_gen_sinboc11_float(ca_codes_f, data_signal, PRN); + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++) { - d_ca_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = static_cast(ca_codes_f[s]); + int32_t tmp_value = static_cast(ca_codes_f[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS) * 2 * (PRN - 1) + s] = tmp_value; } } } 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 203e46357..5f3241696 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 @@ -45,6 +45,11 @@ #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, @@ -53,8 +58,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( Dll_Pll_Conf_Fpga trk_param_fpga = Dll_Pll_Conf_Fpga(); DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - std::string default_item_type = "gr_complex"; - std::string item_type = configuration->property(role + ".item_type", default_item_type); int32_t fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 12000000); int32_t fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); trk_param_fpga.fs_in = fs_in; @@ -65,12 +68,71 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.dump_filename = dump_filename; bool dump_mat = configuration->property(role + ".dump_mat", true); trk_param_fpga.dump_mat = dump_mat; + trk_param_fpga.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param_fpga.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: Gal. E5a. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); - if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } trk_param_fpga.pll_bw_hz = pll_bw_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); - if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } trk_param_fpga.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param_fpga.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param_fpga.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param_fpga.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param_fpga.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param_fpga.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param_fpga.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param_fpga.pull_in_time_s = pull_in_time_s; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); trk_param_fpga.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); @@ -106,17 +168,30 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( char sig_[3] = "5X"; std::memcpy(trk_param_fpga.signal, sig_, 3); int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; + 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; + 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; + 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; + if (FLAGS_carrier_lock_th != 0.85) + { + carrier_lock_th = FLAGS_carrier_lock_th; + } trk_param_fpga.carrier_lock_th = carrier_lock_th; + d_data_codes = nullptr; // FPGA configuration parameters @@ -143,19 +218,41 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( for (uint32_t PRN = 1; PRN <= GALILEO_E5A_NUMBER_OF_CODES; PRN++) { galileo_e5_a_code_gen_complex_primary(aux_code, PRN, const_cast(sig_)); + if (trk_param_fpga.track_pilot) { + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(aux_code[s].imag()); - d_data_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(aux_code[s].real()); + int32_t tmp_value = static_cast(aux_code[s].imag()); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; + + tmp_value = static_cast(aux_code[s].real()); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; + d_data_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; } } else { + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(aux_code[s].real()); + int32_t tmp_value = static_cast(aux_code[s].real()); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; } } } 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 eeaa1da6c..1950e4630 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 @@ -99,7 +99,6 @@ private: std::string role_; uint32_t in_streams_; uint32_t out_streams_; - int32_t* d_ca_codes; int32_t* d_data_codes; bool d_track_pilot; 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 bc0cfa72c..e359dba88 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 @@ -48,8 +48,11 @@ #include // for memcpy #include -#define NUM_PRNs 32 +#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, @@ -62,6 +65,17 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( 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); trk_param_fpga.fs_in = fs_in; + trk_param_fpga.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param_fpga.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + 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"; @@ -70,15 +84,64 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( bool dump_mat = configuration->property(role + ".dump_mat", true); trk_param_fpga.dump_mat = dump_mat; float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } trk_param_fpga.pll_bw_hz = pll_bw_hz; float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); trk_param_fpga.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); trk_param_fpga.dll_bw_narrow_hz = dll_bw_narrow_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); - if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } trk_param_fpga.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param_fpga.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param_fpga.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param_fpga.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param_fpga.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param_fpga.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param_fpga.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param_fpga.pull_in_time_s = pull_in_time_s; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param_fpga.early_late_space_chips = early_late_space_chips; float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); @@ -113,16 +176,28 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( char sig_[3] = "1C"; std::memcpy(trk_param_fpga.signal, sig_, 3); int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; + 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; + 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; + 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; + 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; // FPGA configuration parameters @@ -138,6 +213,18 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l1_ca_code_gen_int(&d_ca_codes[(int32_t(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0); + + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA + for (uint32_t k = 0; k < GPS_L1_CA_CODE_LENGTH_CHIPS; k++) + { + int32_t tmp_value = d_ca_codes[(int32_t(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1) + k]; + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[(int32_t(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1) + k] = tmp_value; + } } trk_param_fpga.ca_codes = d_ca_codes; trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; 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 b733b07f3..1dca0c03a 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 @@ -50,7 +50,12 @@ #include // for memcpy #include -#define NUM_PRNs 32 +#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( @@ -70,11 +75,27 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.dump_filename = dump_filename; bool dump_mat = configuration->property(role + ".dump_mat", true); trk_param_fpga.dump_mat = dump_mat; + trk_param_fpga.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param_fpga.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: GPS L5. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); - if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + if (FLAGS_pll_bw_hz != 0.0) + { + pll_bw_hz = static_cast(FLAGS_pll_bw_hz); + } trk_param_fpga.pll_bw_hz = pll_bw_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); - if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + if (FLAGS_dll_bw_hz != 0.0) + { + dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + } trk_param_fpga.dll_bw_hz = dll_bw_hz; float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); trk_param_fpga.pll_bw_narrow_hz = pll_bw_narrow_hz; @@ -82,6 +103,49 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.dll_bw_narrow_hz = dll_bw_narrow_hz; float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param_fpga.early_late_space_chips = early_late_space_chips; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param_fpga.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param_fpga.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param_fpga.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param_fpga.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param_fpga.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param_fpga.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param_fpga.pull_in_time_s = pull_in_time_s; + int32_t vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5I_CODE_RATE_HZ) / static_cast(GPS_L5I_CODE_LENGTH_CHIPS))); trk_param_fpga.vector_length = vector_length; int32_t extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); @@ -111,16 +175,28 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( char sig_[3] = "L5"; std::memcpy(trk_param_fpga.signal, sig_, 3); int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; + 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; + 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; + 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; + if (FLAGS_carrier_lock_th != 0.85) + { + carrier_lock_th = FLAGS_carrier_lock_th; + } trk_param_fpga.carrier_lock_th = carrier_lock_th; // FPGA configuration parameters @@ -163,18 +239,40 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( gps_l5q_code_gen_float(tracking_code, PRN); gps_l5i_code_gen_float(data_code, PRN); + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(tracking_code[s]); - d_data_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(data_code[s]); + int32_t tmp_value = static_cast(tracking_code[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; + + tmp_value = static_cast(data_code[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; + d_data_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; } } else { gps_l5i_code_gen_float(tracking_code, PRN); + + // The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA for (uint32_t s = 0; s < code_length_chips; s++) { - d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = static_cast(tracking_code[s]); + int32_t tmp_value = static_cast(tracking_code[s]); + if (tmp_value < 0) + { + tmp_value = 0; + } + tmp_value = tmp_value | LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY; + d_ca_codes[static_cast(code_length_chips) * (PRN - 1) + s] = tmp_value; } } } 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 481a82de8..360241991 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 @@ -80,15 +80,17 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & this->message_port_register_in(pmt::mp("preamble_samplestamp")); // Telemetry message port input this->message_port_register_in(pmt::mp("telemetry_to_trk")); - //todo: Implement the telemetry_to_trk handler in the same way the software version of tracking + 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_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_gps_l1ca_preambles_symbols = nullptr; + d_preambles_symbols = nullptr; + d_preamble_length_symbols = 0; signal_type = std::string(trk_parameters.signal); std::map map_signal_pretty_name; @@ -124,7 +126,8 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & uint16_t preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE; // preamble bits to sampled symbols - d_gps_l1ca_preambles_symbols = static_cast(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int32_t), volk_gnsssdr_get_alignment())); + 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) { @@ -132,17 +135,17 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & { if (preambles_bit == 1) { - d_gps_l1ca_preambles_symbols[n] = 1; + d_preambles_symbols[n] = 1; } else { - d_gps_l1ca_preambles_symbols[n] = -1; + d_preambles_symbols[n] = -1; } n++; } } - d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + 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 } else if (signal_type == "2S") { @@ -268,10 +271,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & K_blk_samples = 0.0; // Initialize tracking ========================================== - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); - d_code_loop_filter = Tracking_2nd_DLL_filter(static_cast(d_code_period)); - d_carrier_loop_filter = Tracking_2nd_PLL_filter(static_cast(d_code_period)); + d_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); if (d_veml) { @@ -326,6 +328,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } // --- Initializations --- + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); // Initial code frequency basis of NCO d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) @@ -340,6 +343,8 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_current_prn_length_samples = static_cast(trk_parameters.vector_length); d_next_prn_length_samples = d_current_prn_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 @@ -370,13 +375,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & if (trk_parameters.smoother_length > 0) { - d_carr_ph_history.resize(trk_parameters.smoother_length * 2); - d_code_ph_history.resize(trk_parameters.smoother_length * 2); + d_carr_ph_history.set_capacity(trk_parameters.smoother_length * 2); + d_code_ph_history.set_capacity(trk_parameters.smoother_length * 2); } else { - d_carr_ph_history.resize(1); - d_code_ph_history.resize(1); + d_carr_ph_history.set_capacity(1); + d_code_ph_history.set_capacity(1); } d_dump = trk_parameters.dump; @@ -425,9 +430,45 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_sample_counter_next = 0ULL; } +void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type() == typeid(int)) + { + int tlm_event; + tlm_event = boost::any_cast(pmt::any_ref(msg)); + + switch (tlm_event) + { + case 1: //tlm fault in current channel + { + DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; + gr::thread::scoped_lock lock(d_setlock); + d_carrier_lock_fail_counter = 10000; //force loss-of-lock condition + break; + } + default: + { + break; + } + } + } + } + catch (boost::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any cast!"; + } +} + void dll_pll_veml_tracking_fpga::start_tracking() { + // all the calculations that do not require the data from the acquisition module are moved to the + // set_gnss_synchro command, which is received with a valid PRN before the acquisition module starts the + // acquisition process. This is done to minimize the time between the end of the acquisition process and + // the beginning of the tracking process. + // correct the code phase according to the delay between acq and trk d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; @@ -435,75 +476,13 @@ void dll_pll_veml_tracking_fpga::start_tracking() d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; - d_carrier_phase_rate_step_rad = 0.0; - d_carr_ph_history.clear(); - d_code_ph_history.clear(); - // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter - if (systemName == "GPS" and signal_type == "L5") - { - if (trk_parameters.track_pilot) - { - d_Prompt_Data[0] = gr_complex(0.0, 0.0); - } - } - else if (systemName == "Galileo" and signal_type == "1B") - { - if (trk_parameters.track_pilot) - { - d_Prompt_Data[0] = gr_complex(0.0, 0.0); - } - } - else if (systemName == "Galileo" and signal_type == "5X") - { - 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); - } - } - std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + // filter initialization + d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter - d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0.0; - d_rem_carr_phase_rad = 0.0; - d_rem_code_phase_chips = 0.0; - d_acc_carrier_phase_rad = 0.0; - d_cn0_estimation_counter = 0; - d_carrier_lock_test = 1.0; - d_CN0_SNV_dB_Hz = 0.0; - - if (d_veml) - { - d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); - } - - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); - d_carrier_loop_filter.set_pdi(static_cast(d_code_period)); - d_code_loop_filter.set_pdi(static_cast(d_code_period)); - - // DEBUG OUTPUT - std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - multicorrelator_fpga->set_local_code_and_taps(d_local_code_shift_chips, d_prompt_data_shift, d_acquisition_gnss_synchro->PRN); // enable tracking pull-in d_state = 1; - d_cloop = true; - d_Prompt_buffer_deque.clear(); - d_last_prompt = gr_complex(0.0, 0.0); } @@ -511,7 +490,7 @@ dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() { if (signal_type == "1C") { - volk_gnsssdr_free(d_gps_l1ca_preambles_symbols); + volk_gnsssdr_free(d_preambles_symbols); } if (d_dump_file.is_open()) @@ -557,7 +536,8 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() int32_t corr_value = 0; for (uint32_t i = 0; i < d_secondary_code_length; i++) { - if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping + 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,17 +565,14 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() { return true; } - else - { - return false; - } + + return false; } bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integration_time_s) { // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < trk_parameters.cn0_samples) { // fill buffer with prompt correlator output values @@ -611,13 +588,19 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra // Carrier lock indicator d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + if (!d_pull_in_transitory) { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + 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) { @@ -641,10 +624,10 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra // - updated remnant code phase in samples (d_rem_code_phase_samples) // - d_code_freq_chips // - d_carrier_doppler_hz -//void dll_pll_veml_tracking_fpga::do_correlation_step(const gr_complex *input_samples) void dll_pll_veml_tracking_fpga::do_correlation_step(void) { - // // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_fpga->Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, @@ -660,22 +643,48 @@ 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 + d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; } else { // Secondary code acquired. No symbols transition should be present in the signal + d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; } - // Carrier discriminator filter - d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; + 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_P_accu_old = d_P_accu; + // Carrier discriminator filter + if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) + { + //pure FLL, disable PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); + } + else + { + //FLL-aided PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); + } + } + else + { + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(0, d_carr_phase_error_hz, d_current_correlation_time_s); + } + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_carr_error_filt_hz; + + // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; + // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -687,7 +696,7 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] } // Code discriminator filter - d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] + 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; @@ -697,13 +706,20 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() void dll_pll_veml_tracking_fpga::clear_tracking_vars() { std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); - if (trk_parameters.track_pilot) d_Prompt_Data[0] = gr_complex(0.0, 0.0); + if (trk_parameters.track_pilot) + { + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + d_P_accu_old = gr_complex(0.0, 0.0); + d_carr_phase_error_hz = 0.0; + d_carr_freq_error_hz = 0.0; d_carr_error_hz = 0.0; d_carr_error_filt_hz = 0.0; d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; - d_Prompt_buffer_deque.clear(); + d_Prompt_circular_buffer.clear(); + //d_Prompt_buffer_deque.clear(); d_last_prompt = gr_complex(0.0, 0.0); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -722,6 +738,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples = T_prn_seconds * trk_parameters.fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples; + //d_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 //################### PLL COMMANDS ################################################# @@ -736,22 +753,26 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() double tmp_cp1 = 0.0; double tmp_cp2 = 0.0; double tmp_samples = 0.0; - for (uint32_t k = 0; k < trk_parameters.smoother_length; k++) + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) { - tmp_cp1 += d_carr_ph_history.at(k).first; - tmp_cp2 += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first; - tmp_samples += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second; + tmp_cp1 += d_carr_ph_history[k].first; + tmp_cp2 += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; } tmp_cp1 /= static_cast(trk_parameters.smoother_length); tmp_cp2 /= static_cast(trk_parameters.smoother_length); d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } + //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator + //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); //################### DLL COMMANDS ################################################# @@ -765,11 +786,11 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() double tmp_cp1 = 0.0; double tmp_cp2 = 0.0; double tmp_samples = 0.0; - for (uint32_t k = 0; k < trk_parameters.smoother_length; k++) + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) { - tmp_cp1 += d_code_ph_history.at(k).first; - tmp_cp2 += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first; - tmp_samples += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second; + tmp_cp1 += d_code_ph_history[k].first; + tmp_cp2 += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; } tmp_cp1 /= static_cast(trk_parameters.smoother_length); tmp_cp2 /= static_cast(trk_parameters.smoother_length); @@ -827,9 +848,13 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() } // If tracking pilot, disable Costas loop if (trk_parameters.track_pilot) - d_cloop = false; + { + d_cloop = false; + } else - d_cloop = true; + { + d_cloop = true; + } } @@ -927,7 +952,8 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands - tmp_float = d_carr_error_hz; + 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)); @@ -1221,6 +1247,8 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { try { + //trk_parameters.dump_filename.append(boost::lexical_cast(d_channel)); + //trk_parameters.dump_filename.append(".dat"); d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str(); @@ -1237,6 +1265,86 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) 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) + { + //std::cout << "Acquisition is about to start " << std::endl; + + // When using the FPGA the SW only reads the sample counter during active tracking in order to spare CPU clock cycles. + d_sample_counter = 0; + d_sample_counter_next = 0; + + d_carrier_phase_rate_step_rad = 0.0; + + d_code_ph_history.clear(); + + // DLL/PLL filter initialization + d_code_loop_filter.initialize(); // initialize the code filter + + d_carr_ph_history.clear(); + + if (systemName == "GPS" and signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + } + else if (systemName == "Galileo" and signal_type == "1B") + { + if (trk_parameters.track_pilot) + { + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + } + else if (systemName == "Galileo" and signal_type == "5X") + { + 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); + } + } + + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_cn0_estimation_counter = 0; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + } + + d_current_correlation_time_s = d_code_period; + + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_hz); + d_code_loop_filter.set_update_interval(d_code_period); + + multicorrelator_fpga->set_local_code_and_taps(d_local_code_shift_chips, d_prompt_data_shift, d_acquisition_gnss_synchro->PRN); + + d_pull_in_transitory = true; + + //d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); + + d_cloop = true; + + d_Prompt_circular_buffer.clear(); + } } @@ -1262,15 +1370,30 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_current_prn_length_samples = d_next_prn_length_samples; + + if (d_pull_in_transitory == true) + { + if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking + { + 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; + } + } + } switch (d_state) { case 0: // Standby - Consume samples at full throttle, do nothing { - return 0; + *out[0] = *d_acquisition_gnss_synchro; + usleep(1000); + return 1; break; } 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; @@ -1304,7 +1427,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Tracking_sample_counter = absolute_samples_offset; d_sample_counter_next = d_sample_counter; - // Signal alignment (skip samples until the incoming signal is aligned with local replica) // Doppler effect Fd = (C / (C + Vr)) * F double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; @@ -1325,7 +1447,12 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); d_state = 2; + // 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; @@ -1470,9 +1597,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (current_synchro_data.Flag_valid_symbol_output) { current_synchro_data.fs = static_cast(trk_parameters.fs_in); - // two choices for the reporting of the sample counter: - // either the sample counter position that should be (d_sample_counter_next) - //or the sample counter corresponding to the number of samples that the FPGA actually consumed. current_synchro_data.Tracking_sample_counter = d_sample_counter_next; *out[0] = current_synchro_data; return 1; @@ -1515,17 +1639,20 @@ void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data) if (d_secondary) { // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + d_Prompt_circular_buffer.push_back(*d_Prompt); + //d_Prompt_buffer_deque.push_back(*d_Prompt); + //if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + 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; } - - d_Prompt_buffer_deque.pop_front(); + //d_Prompt_buffer_deque.pop_front(); } } else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change @@ -1538,20 +1665,24 @@ void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data) int32_t corr_value = 0; if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync)) { - for (uint32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) + int i = 0; + for (const auto &iter : d_symbol_history) { - if (d_symbol_history.at(i) < 0) // symbols clipping + if (iter < 0.0) // symbols clipping { - corr_value -= d_gps_l1ca_preambles_symbols[i]; + corr_value -= d_preambles_symbols[i]; } else { - corr_value += d_gps_l1ca_preambles_symbols[i]; + corr_value += d_preambles_symbols[i]; } + i++; } } if (corr_value == GPS_CA_PREAMBLE_LENGTH_SYMBOLS) { + 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; } else @@ -1598,6 +1729,7 @@ void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data) 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; @@ -1612,17 +1744,17 @@ void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data) d_P_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); - d_last_prompt = gr_complex(0.0, 0.0); - d_Prompt_buffer_deque.clear(); + d_Prompt_circular_buffer.clear(); d_current_symbol = 0; + d_last_prompt = gr_complex(0.0, 0.0); + //d_Prompt_buffer_deque.clear(); if (d_enable_extended_integration) { // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; - float new_correlation_time = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time); - d_code_loop_filter.set_pdi(new_correlation_time); + d_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 @@ -1631,8 +1763,9 @@ void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data) << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; // Set narrow taps delay values [chips] - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_narrow_hz); + 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); 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 681573639..349db24f0 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 @@ -33,20 +33,20 @@ #define GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H #include "dll_pll_conf_fpga.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" +#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter +#include "tracking_loop_filter.h" // for DLL filter #include -#include -#include +#include // for boost::shared_ptr +#include // for block #include // for gr_complex -#include // for gr_vector_const_void_star +#include // for gr_vector_int, gr_vector... #include // for pmt_t -#include -#include // for deque -#include // for ofstream -#include // for shared_ptr +#include // for int32_t +#include // for deque +#include // for string, ofstream +#include // for shared_ptr #include -#include +#include // for pair class Fpga_Multicorrelator_8sc; class Gnss_Synchro; @@ -77,7 +77,7 @@ public: 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); @@ -115,23 +115,20 @@ private: std::string *d_secondary_code_string; std::string signal_pretty_name; - int32_t *d_gps_l1ca_preambles_symbols; + int32_t *d_preambles_symbols; + int32_t d_preamble_length_symbols; boost::circular_buffer d_symbol_history; - //tracking state machine + // tracking state machine int32_t d_state; - //Integration period in samples + + // Integration period in samples int32_t d_correlation_length_ms; int32_t d_n_correlator_taps; float *d_local_code_shift_chips; float *d_prompt_data_shift; std::shared_ptr multicorrelator_fpga; - /* TODO: currently the multicorrelator does not support adding extra correlator - with different local code, thus we need extra multicorrelator instance. - Implement this functionality inside multicorrelator class - as an enhancement to increase the performance - */ gr_complex *d_correlator_outs; gr_complex *d_Very_Early; gr_complex *d_Early; @@ -146,6 +143,7 @@ private: gr_complex d_VE_accu; gr_complex d_E_accu; gr_complex d_P_accu; + gr_complex d_P_accu_old; gr_complex d_L_accu; gr_complex d_VL_accu; gr_complex d_last_prompt; @@ -163,14 +161,18 @@ private: float d_rem_carr_phase_rad; // PLL and DLL filter library - Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; + Tracking_loop_filter d_code_loop_filter; + Tracking_FLL_PLL_filter d_carrier_loop_filter; // acquisition double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // tracking vars + bool d_pull_in_transitory; + double d_current_correlation_time_s; + double d_carr_phase_error_hz; + double d_carr_freq_error_hz; double d_carr_error_hz; double d_carr_error_filt_hz; double d_code_error_chips; @@ -194,11 +196,12 @@ private: // 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; + //std::deque d_carrier_lock_detector_queue; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; - std::deque d_Prompt_buffer_deque; + boost::circular_buffer d_Prompt_circular_buffer; + //std::deque d_Prompt_buffer_deque; gr_complex *d_Prompt_buffer; // file dump diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 14ec6ccab..8ad8f3a13 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -44,6 +44,15 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() dump = false; dump_mat = true; dump_filename = std::string("./dll_pll_dump.dat"); + enable_fll_pull_in = false; + enable_fll_steady_state = false; + pull_in_time_s = 2; + fll_filter_order = 1; + pll_filter_order = 3; + dll_filter_order = 2; + fll_bw_hz = 35.0; + pll_pull_in_bw_hz = 50.0; + dll_pull_in_bw_hz = 3.0; pll_bw_hz = 35.0; dll_bw_hz = 2.0; pll_bw_narrow_hz = 5.0; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index 33dd901d7..a614d32ff 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -42,11 +42,22 @@ 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 + int pll_filter_order; + int dll_filter_order; + + double fs_in; uint32_t vector_length; bool dump; bool dump_mat; std::string dump_filename; + float pll_pull_in_bw_hz; + float dll_pull_in_bw_hz; + float fll_bw_hz; float pll_bw_hz; float dll_bw_hz; float pll_bw_narrow_hz; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index b832a828c..f674da1d2 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -51,9 +51,9 @@ #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_NBITS 32 -#define PHASE_CARR_NBITS_INT 1 -#define PHASE_CARR_NBITS_FRAC PHASE_CARR_NBITS - PHASE_CARR_NBITS_INT +#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 @@ -118,6 +118,7 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, 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; DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; } @@ -171,9 +172,9 @@ void Fpga_Multicorrelator_8sc::set_output_vectors(gr_complex *corr_out, gr_compl } -void Fpga_Multicorrelator_8sc::update_local_code(float rem_code_phase_chips) +void Fpga_Multicorrelator_8sc::update_local_code() { - d_rem_code_phase_chips = rem_code_phase_chips; + //d_rem_code_phase_chips = rem_code_phase_chips; Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(); Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(); } @@ -188,11 +189,12 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( float code_phase_rate_step_chips __attribute__((unused)), int32_t signal_length_samples) { - update_local_code(rem_code_phase_chips); + d_rem_code_phase_chips = rem_code_phase_chips; d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad; d_code_phase_step_chips = code_phase_step_chips; d_phase_step_rad = phase_step_rad; d_correlator_length_samples = signal_length_samples; + Fpga_Multicorrelator_8sc::update_local_code(); Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga(); @@ -298,88 +300,72 @@ uint32_t Fpga_Multicorrelator_8sc::fpga_acquisition_test_register( void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PRN) { uint32_t k; - uint32_t code_chip; - uint32_t select_pilot_corelator = LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; - for (k = 0; k < d_code_length_chips * d_code_samples_per_chip; k++) + for (k = 0; k < d_code_length_samples; k++) { - if (d_ca_codes[((int32_t(d_code_length_chips)) * d_code_samples_per_chip * (PRN - 1)) + k] == 1) - { - code_chip = 1; - } - else - { - code_chip = 0; - } - - // copy the local code to the FPGA memory one by one - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | code_chip; // | select_fpga_correlator; + 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; - for (k = 0; k < d_code_length_chips * d_code_samples_per_chip; k++) + for (k = 0; k < d_code_length_samples; k++) { - if (d_data_codes[((int32_t(d_code_length_chips)) * d_code_samples_per_chip * (PRN - 1)) + k] == 1) - { - code_chip = 1; - } - else - { - code_chip = 0; - } - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | code_chip | select_pilot_corelator; + 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 } void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) { - float temp_calculation; - int32_t i; + float frac_part; // decimal part + int32_t dec_part; // fractional part - for (i = 0; i < d_n_correlators; i++) + for (uint32_t i = 0; i < d_n_correlators; i++) { - temp_calculation = floor(d_shifts_chips[i] - d_rem_code_phase_chips); + dec_part = floor(d_shifts_chips[i] - d_rem_code_phase_chips); - if (temp_calculation < 0) + if (dec_part < 0) { - temp_calculation = temp_calculation + (d_code_length_chips * d_code_samples_per_chip); // % operator does not work as in Matlab with negative numbers + dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers } - d_initial_index[i] = static_cast((static_cast(temp_calculation)) % (d_code_length_chips * d_code_samples_per_chip)); - temp_calculation = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, 1.0); - if (temp_calculation < 0) + d_initial_index[i] = dec_part; + + + frac_part = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, 1.0); + if (frac_part < 0) { - temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers + 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 * temp_calculation)); + d_initial_interp_counter[i] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); } if (d_track_pilot) { - temp_calculation = floor(d_prompt_data_shift[0] - d_rem_code_phase_chips); + dec_part = floor(d_prompt_data_shift[0] - d_rem_code_phase_chips); - if (temp_calculation < 0) + if (dec_part < 0) { - temp_calculation = temp_calculation + (d_code_length_chips * d_code_samples_per_chip); // % operator does not work as in Matlab with negative numbers + dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers } - d_initial_index[d_n_correlators] = static_cast((static_cast(temp_calculation)) % (d_code_length_chips * d_code_samples_per_chip)); - temp_calculation = fmod(d_prompt_data_shift[0] - d_rem_code_phase_chips, 1.0); - if (temp_calculation < 0) + d_initial_index[d_n_correlators] = dec_part; + + frac_part = fmod(d_prompt_data_shift[0] - d_rem_code_phase_chips, 1.0); + if (frac_part < 0) { - temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers + 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 * temp_calculation)); + d_initial_interp_counter[d_n_correlators] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); } } void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void) { - int32_t i; - for (i = 0; i < d_n_correlators; i++) + 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]; @@ -389,8 +375,6 @@ void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void) 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[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_chips * d_code_samples_per_chip) - 1; // number of samples - 1 } @@ -399,34 +383,22 @@ 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)); - if (d_code_phase_step_chips > 1.0) - { - std::cout << "Warning : d_code_phase_step_chips = " << d_code_phase_step_chips << " cannot be bigger than one" << std::endl; - } if (d_rem_carrier_phase_in_rad > M_PI) { - d_rem_carrier_phase_in_rad_temp = -2 * M_PI + d_rem_carrier_phase_in_rad; + d_rem_carrier_phase_in_rad_temp = -TWO_PI + d_rem_carrier_phase_in_rad; } else if (d_rem_carrier_phase_in_rad < -M_PI) { - d_rem_carrier_phase_in_rad_temp = 2 * M_PI + d_rem_carrier_phase_in_rad; + d_rem_carrier_phase_in_rad_temp = TWO_PI + d_rem_carrier_phase_in_rad; } else { d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad; } - d_rem_carr_phase_rad_int = static_cast(roundf((fabs(d_rem_carrier_phase_in_rad_temp) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); - if (d_rem_carrier_phase_in_rad_temp < 0) - { - d_rem_carr_phase_rad_int = -d_rem_carr_phase_rad_int; - } - d_phase_step_rad_int = static_cast(roundf((fabs(d_phase_step_rad) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); // the FPGA accepts a range for the phase step between -pi and +pi - if (d_phase_step_rad < 0) - { - d_phase_step_rad_int = -d_phase_step_rad_int; - } + 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 } @@ -460,9 +432,8 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) { int32_t readval_real; int32_t readval_imag; - int32_t k; - for (k = 0; k < d_n_correlators; k++) + 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]; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index df060c362..c97506839 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -77,7 +77,7 @@ public: void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data); void set_local_code_and_taps( float *shifts_chips, float *prompt_data_shift, int32_t PRN); - void update_local_code(float rem_code_phase_chips); + void update_local_code(); void Carrier_wipeoff_multicorrelator_resampler( float rem_carrier_phase_in_rad, float phase_step_rad, float carrier_phase_rate_step_rad, @@ -96,15 +96,16 @@ private: gr_complex *d_Prompt_Data; float *d_shifts_chips; float *d_prompt_data_shift; - int32_t d_code_length_chips; - int32_t d_n_correlators; // number of correlators + uint32_t d_code_length_chips; + uint32_t d_code_length_samples; + uint32_t d_n_correlators; // number of correlators // data related to the hardware module and the driver int32_t d_device_descriptor; // driver descriptor volatile uint32_t *d_map_base; // driver memory map // configuration data received from the interface - uint32_t d_channel; // channel number + uint32_t d_channel; // channel number uint32_t d_correlator_length_samples; float d_rem_code_phase_chips; float d_code_phase_step_chips; diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.cc b/src/core/libs/gnss_sdr_fpga_sample_counter.cc index 95baf7a24..85c6f6018 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.cc +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.cc @@ -2,11 +2,12 @@ * \file gnss_sdr_fpga_sample_counter.cc * \brief Simple block to report the current receiver time based on the output * of the tracking or telemetry blocks + * \author Marc Majoral 2019. mmajoral(at)cttc.es * \author Javier Arribas 2018. jarribas(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -69,19 +70,17 @@ gnss_sdr_fpga_sample_counter::gnss_sdr_fpga_sample_counter( interval_ms = _interval_ms; fs = _fs; samples_per_output = std::round(fs * static_cast(interval_ms) / 1e3); - //todo: Load here the hardware counter register with this amount of samples. It should produce an - //interrupt every samples_per_output count. - //The hardware timer must keep always interrupting the PS. It must not wait for the interrupt to - //be served. open_device(); is_open = true; sample_counter = 0ULL; + last_sample_counter = 0ULL; current_T_rx_ms = 0; current_s = 0; current_m = 0; current_h = 0; current_days = 0; - report_interval_ms = 1000; // default reporting 1 second + report_interval_ms = 1000; // default reporting 1 second + samples_per_report = std::round(fs * static_cast(report_interval_ms) / 1e3); flag_enable_send_msg = false; // enable it for reporting time with asynchronous message flag_m = false; flag_h = false; @@ -105,11 +104,9 @@ gnss_sdr_fpga_sample_counter::~gnss_sdr_fpga_sample_counter() } -// Called by gnuradio to enable drivers, etc for i/o devices. +// Called by GNU Radio to enable drivers, etc for i/o devices. bool gnss_sdr_fpga_sample_counter::start() { - //todo: place here the RE-INITIALIZATION routines. This function will be called by GNURadio at every start of the flowgraph. - // configure the number of samples per output in the FPGA and enable the interrupts configure_samples_per_output(samples_per_output); @@ -118,11 +115,9 @@ bool gnss_sdr_fpga_sample_counter::start() } -// Called by GNURadio to disable drivers, etc for i/o devices. +// Called by GNU Radio to disable drivers, etc for i/o devices. bool gnss_sdr_fpga_sample_counter::stop() { - //todo: place here the routines to stop the associated hardware (if needed).This function will be called by GNURadio at every stop of the flowgraph. - // return true if everything is ok. close_device(); is_open = false; return true; @@ -194,65 +189,31 @@ void gnss_sdr_fpga_sample_counter::close_device() } -uint32_t gnss_sdr_fpga_sample_counter::wait_for_interrupt_and_read_counter() -{ - int32_t irq_count; - ssize_t nb; - int32_t counter; - - // enable interrupts - int32_t reenable = 1; - ssize_t nbytes = TEMP_FAILURE_RETRY(write(fd, reinterpret_cast(&reenable), sizeof(int32_t))); - if (nbytes != sizeof(int32_t)) - { - std::cerr << "Error enabling interruptions in the FPGA." << std::endl; - } - - // wait for interrupt - nb = read(fd, &irq_count, sizeof(irq_count)); - if (nb != sizeof(irq_count)) - { - std::cout << "FPGA sample counter module read failed to retrieve 4 bytes!" << std::endl; - std::cout << "FPGA sample counter module interrupt number " << irq_count << std::endl; - } - - // it is a rising edge interrupt, the interrupt does not need to be acknowledged - //map_base[1] = 0; // writing anything to reg 1 acknowledges the interrupt - - // add number of passed samples or read the current counter value for more accuracy - counter = samples_per_output; //map_base[0]; - return counter; -} - - int gnss_sdr_fpga_sample_counter::general_work(int noutput_items __attribute__((unused)), __attribute__((unused)) gr_vector_int &ninput_items, __attribute__((unused)) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - //todo: Call here a function that waits for an interrupt. Do not open a thread, - //it must be a simple call to a BLOCKING function. - // The function will return the actual absolute sample count of the internal counter of the timmer. - // store the sample count in class member sample_counter - // Possible problem: what happen if the PS is overloaded and gnuradio does not call this function - // with the sufficient rate to catch all the interrupts in the counter. To be evaluated later. + wait_for_interrupt(); - uint32_t counter = wait_for_interrupt_and_read_counter(); - uint64_t samples_passed = 2 * static_cast(samples_per_output) - static_cast(counter); // ellapsed samples - // Note: at this moment the sample counter is implemented as a sample counter that decreases to zero and then it is automatically - // reloaded again and keeps counter. It is done in this way to minimize the logic in the FPGA and maximize the FPGA clock performance - // (it takes less resources and latency in the FPGA to compare a number against a fixed value like zero than to compare it to a programmable - // variable number). + uint64_t sample_counter_tmp, sample_counter_msw_tmp; + sample_counter_tmp = map_base[0]; + sample_counter_msw_tmp = map_base[1]; + sample_counter_msw_tmp = sample_counter_msw_tmp << 32; + sample_counter_tmp = sample_counter_tmp + sample_counter_msw_tmp; // 2^32 + sample_counter = sample_counter_tmp; - sample_counter = sample_counter + samples_passed; //samples_per_output; auto *out = reinterpret_cast(output_items[0]); out[0] = Gnss_Synchro(); out[0].Flag_valid_symbol_output = false; out[0].Flag_valid_word = false; out[0].Channel_ID = -1; out[0].fs = fs; - if ((current_T_rx_ms % report_interval_ms) == 0) + + if ((sample_counter - last_sample_counter) > samples_per_report) { + last_sample_counter = sample_counter; + current_s++; if ((current_s % 60) == 0) { @@ -310,7 +271,25 @@ int gnss_sdr_fpga_sample_counter::general_work(int noutput_items __attribute__(( } } out[0].Tracking_sample_counter = sample_counter; - //current_T_rx_ms = (sample_counter * 1000) / samples_per_output; current_T_rx_ms = interval_ms * (sample_counter) / samples_per_output; return 1; } + + +void gnss_sdr_fpga_sample_counter::wait_for_interrupt() +{ + int32_t irq_count; + ssize_t nb; + + // enable interrupts + int32_t reenable = 1; + write(fd, reinterpret_cast(&reenable), sizeof(int32_t)); + + // wait for interrupt + nb = read(fd, &irq_count, sizeof(irq_count)); + if (nb != sizeof(irq_count)) + { + std::cout << "fpga sample counter module read failed to retrieve 4 bytes!" << std::endl; + std::cout << "fpga sample counter module interrupt number " << irq_count << 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 1f5db0b2d..70df49283 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.h +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.h @@ -55,11 +55,15 @@ private: void close_device(void); void open_device(void); bool start(); + bool stop(); - uint32_t wait_for_interrupt_and_read_counter(void); + void wait_for_interrupt(void); uint32_t samples_per_output; + uint32_t samples_per_report; double fs; uint64_t sample_counter; + uint64_t last_sample_counter; + uint32_t interval_ms; uint64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run uint32_t current_s; // Receiver time in seconds, modulo 60 @@ -70,6 +74,7 @@ private: bool flag_days; // True if the receiver has been running for at least 1 day uint32_t current_days; // Receiver time in days since the beginning of the run int32_t report_interval_ms; + bool flag_enable_send_msg; int32_t fd; // driver descriptor volatile uint32_t *map_base; // driver memory map