implemented double acquisition for the FPGA

src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc

@@ -161,6 +161,10 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga(
 
     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);
+
     // reference for the FPGA FFT-IFFT attenuation factor
     acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14);
 

src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc

@@ -162,6 +162,10 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf
     // reference for the FPGA FFT-IFFT attenuation factor
     acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14);
 
+    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);
+
     acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters);
     DLOG(INFO) << "acquisition(" << acquisition_fpga_->unique_id() << ")";
 

src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc

@@ -143,6 +143,10 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
     // reference for the FPGA FFT-IFFT attenuation factor
     acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14);
 
+    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);
+
     acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters);
     DLOG(INFO) << "acquisition(" << acquisition_fpga_->unique_id() << ")";
 

src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc

@@ -142,6 +142,10 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga(
     // reference for the FPGA FFT-IFFT attenuation factor
     acq_parameters.total_block_exp = configuration_->property(role + ".total_block_exp", 14);
 
+    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);
+
     acquisition_fpga_ = pcps_make_acquisition_fpga(acq_parameters);
     DLOG(INFO) << "acquisition(" << acquisition_fpga_->unique_id() << ")";
 

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc

@@ -75,6 +75,13 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) : gr::block(
 
     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;
+
     acquisition_fpga = std::make_shared<fpga_acquisition>(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);
 }
@@ -104,7 +111,7 @@ void pcps_acquisition_fpga::init()
     d_mag = 0.0;
     d_input_power = 0.0;
 
-    d_num_doppler_bins = static_cast<uint32_t>(std::ceil(static_cast<double>(static_cast<int32_t>(acq_parameters.doppler_max) - static_cast<int32_t>(-acq_parameters.doppler_max)) / static_cast<double>(d_doppler_step))) + 1;
+    d_num_doppler_bins = static_cast<uint32_t>(std::ceil(static_cast<double>(static_cast<int32_t>(d_doppler_max) - static_cast<int32_t>(-d_doppler_max)) / static_cast<double>(d_doppler_step))) + 1;
 
     acquisition_fpga->init();
 }
@@ -167,34 +174,17 @@ void pcps_acquisition_fpga::send_negative_acquisition()
     this->message_port_pub(pmt::mp("events"), pmt::from_long(2));
 }
 
-
+void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, uint32_t doppler_min)
-void pcps_acquisition_fpga::set_active(bool active)
 {
-    d_active = active;
-
-    // initialize acquisition algorithm
     uint32_t indext = 0U;
     float firstpeak = 0.0;
     float secondpeak = 0.0;
     uint32_t total_block_exp;
-
+    uint64_t initial_sample;
-    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->set_doppler_sweep(num_doppler_bins, doppler_step, doppler_min);
     acquisition_fpga->write_local_code();
     acquisition_fpga->set_block_exp(d_total_block_exp);
     acquisition_fpga->run_acquisition();
@@ -207,7 +197,8 @@ void pcps_acquisition_fpga::set_active(bool active)
             d_total_block_exp = total_block_exp;
         }
 
-    doppler = -static_cast<int32_t>(acq_parameters.doppler_max) + d_doppler_step * (d_doppler_index - 1);
+    //doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * (d_doppler_index - 1);
+    doppler = static_cast<int32_t>(doppler_min) + doppler_step * (d_doppler_index - 1);
 
     if (secondpeak > 0)
         {
@@ -239,6 +230,48 @@ void pcps_acquisition_fpga::set_active(bool active)
             d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext);
             d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;  // delay due to the downsampling filter in the acquisition
         }
+}
+
+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_core(d_num_doppler_bins, d_doppler_step, -d_doppler_max);
+
+    if (!d_make_2_steps)
+        {
+            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
+        {
+            if (d_test_statistics > d_threshold)
+                {
+                    d_doppler_center_step_two = static_cast<float>(d_gnss_synchro->Acq_doppler_hz);
+                    acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast<float>(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2);
 
                     if (d_test_statistics > d_threshold)
                         {
@@ -252,6 +285,36 @@ void pcps_acquisition_fpga::set_active(bool active)
                             d_active = false;
                             send_negative_acquisition();
                         }
+                }
+            else
+                {
+                    d_state = 0;
+                    d_active = false;
+                    send_negative_acquisition();
+                }
+        }
+
+
+    //    if (d_test_statistics > d_threshold)
+    //        {
+    //            //            if (!d_make_2_steps)
+    //            //                {
+    //            d_active = false;
+    //            send_positive_acquisition();
+    //            d_state = 0;  // Positive acquisition
+    //                          //                }
+    //                          //            else
+    //                          //                {
+    //                          //                    d_doppler_center_step_two = static_cast<float>(d_gnss_synchro->Acq_doppler_hz);
+    //                          //                    acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast<float>(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2);
+    //                          //                }
+    //        }
+    //    else
+    //        {
+    //            d_state = 0;
+    //            d_active = false;
+    //            send_negative_acquisition();
+    //        }
 }
 
 

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h

@@ -64,6 +64,9 @@ typedef struct
     float downsampling_factor;
     uint32_t total_block_exp;
     uint32_t excludelimit;
+    bool make_2_steps;
+    uint32_t num_doppler_bins_step2;
+    float doppler_step2;
 } pcpsconf_fpga_t;
 
 class pcps_acquisition_fpga;
@@ -94,6 +97,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, uint32_t doppler_max);
+
     pcpsconf_fpga_t acq_parameters;
     bool d_active;
     float d_threshold;
@@ -104,6 +109,7 @@ private:
     int32_t d_state;
     uint32_t d_channel;
     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;
@@ -115,6 +121,10 @@ private:
 
     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;
 
 public:
     ~pcps_acquisition_fpga();
@@ -187,7 +197,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);
     }
 

src/algorithms/acquisition/libs/fpga_acquisition.cc

@@ -216,12 +216,13 @@ 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, uint32_t doppler_min)
 {
     float phase_step_rad_real;
     float phase_step_rad_int_temp;
     int32_t phase_step_rad_int;
-    int32_t doppler = static_cast<int32_t>(-d_doppler_max);
+    //int32_t doppler = static_cast<int32_t>(-d_doppler_max);
+    int32_t doppler = static_cast<int32_t>(doppler_min);
     float phase_step_rad = GPS_TWO_PI * (doppler) / static_cast<float>(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)
@@ -239,7 +240,8 @@ void fpga_acquisition::set_doppler_sweep(uint32_t num_sweeps)
     d_map_base[3] = phase_step_rad_int;
 
     // repeat the calculation with the doppler step
-    doppler = static_cast<int32_t>(d_doppler_step);
+    //doppler = static_cast<int32_t>(d_doppler_step);
+    doppler = static_cast<int32_t>(doppler_step);
     phase_step_rad = GPS_TWO_PI * (doppler) / static_cast<float>(d_fs_in);
     phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2);
     if (phase_step_rad_real >= 1.0)
@@ -264,30 +266,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<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
-    float phase_step_rad = GPS_TWO_PI * (doppler) / static_cast<float>(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<int32_t>(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)
 {

src/algorithms/acquisition/libs/fpga_acquisition.h

@@ -60,9 +60,9 @@ public:
     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, uint32_t doppler_max);
     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();

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc

@@ -1323,6 +1323,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
                 d_state = 2;
 
                 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;