From 32e1aac6f609bf282582d5bcc8fc741ef504ba3a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 30 Jun 2026 19:53:50 +0200 Subject: [PATCH 1/3] Improve QuickSync acquisition implementation Move QuickSync acquisition temporary buffers out of general_work() and initialize them once in the block constructor. Clear the FFT input buffer directly with std::fill_n() for each Doppler bin instead of recreating and copying d_signal_folded. --- .../pcps_quicksync_acquisition_cc.cc | 32 +++++++------------ .../pcps_quicksync_acquisition_cc.h | 4 ++- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc index 480bae296..fd2390a45 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -63,8 +64,10 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(const Acq_Conf& con d_ifft(gnss_fft_rev_make_unique(d_fft_size)), d_code(d_samples_per_code, lv_cmake(0.0F, 0.0F)), d_fft_codes(d_fft_size), - d_signal_folded(d_fft_size), d_code_folded(d_fft_size, lv_cmake(0.0F, 0.0F)), + d_in_temp(d_samples_per_code * d_folding_factor), + d_in_1code(d_samples_per_code), + d_corr_output(d_samples_per_code), d_magnitude(d_samples_per_code * d_folding_factor), d_corr_output_f(d_folding_factor), d_magnitude_folded(d_fft_size), @@ -189,18 +192,6 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, float magt = 0.0; const auto* in = reinterpret_cast(input_items[0]); // Get the input samples pointer - std::vector in_temp(d_samples_per_code * d_folding_factor); - // Create a signal to store a signal of size 1ms, to perform correlation - // in time. No folding on this data is required - std::vector in_1code(d_samples_per_code); - - // Stores the values of the correlation output between the local code - // and the signal with doppler shift corrected - std::vector corr_output(d_samples_per_code); - - // Stores a copy of the folded version of the signal.This is used for - // the FFT operations in future steps of execution*/ - // gr_complex in_folded[d_fft_size]; float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); d_input_power = 0.0; @@ -234,8 +225,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, // Ensure that the signal is going to start with all samples // at zero. This is done to avoid over accumulation when performing // the folding process to be stored in d_fft_if->get_inbuf() - d_signal_folded = std::vector(d_fft_size, lv_cmake(0.0F, 0.0F)); - std::copy(d_signal_folded.data(), d_signal_folded.data() + d_fft_size, d_fft_if->get_inbuf()); + std::fill_n(d_fft_if->get_inbuf(), d_fft_size, lv_cmake(0.0F, 0.0F)); // Doppler search steps and then multiplication of the incoming // signal with the doppler wipeoffs to eliminate frequency offset @@ -244,7 +234,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, // Perform multiplication of the incoming signal with the // complex exponential vector. This removes the frequency doppler // shift offset - volk_32fc_x2_multiply_32fc(in_temp.data(), in, + volk_32fc_x2_multiply_32fc(d_in_temp.data(), in, d_grid_doppler_wipeoffs[doppler_index].data(), d_samples_per_code * d_folding_factor); @@ -253,8 +243,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, // incoming raw data signal is of d_folding_factor^2 for (int32_t i = 0; i < static_cast(d_folding_factor * d_folding_factor); i++) { - std::transform((in_temp.data() + i * d_fft_size), - (in_temp.data() + ((i + 1) * d_fft_size)), + std::transform((d_in_temp.data() + i * d_fft_size), + (d_in_temp.data() + ((i + 1) * d_fft_size)), d_fft_if->get_inbuf(), d_fft_if->get_inbuf(), std::plus()); @@ -311,18 +301,18 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items, { // Copy a signal of 1 code length into suggested buffer. // The copied signal must have doppler effect corrected - std::copy_n(&in_temp[d_possible_delay[i]], d_samples_per_code, in_1code.data()); + std::copy_n(&d_in_temp[d_possible_delay[i]], d_samples_per_code, d_in_1code.data()); // Perform multiplication of the unmodified local // generated code with the incoming signal with doppler // effect corrected and accumulates its value. This // is indeed correlation in time for an specific value // of a shift - volk_32fc_x2_multiply_32fc(corr_output.data(), in_1code.data(), d_code.data(), d_samples_per_code); + volk_32fc_x2_multiply_32fc(d_corr_output.data(), d_in_1code.data(), d_code.data(), d_samples_per_code); for (int32_t j = 0; j < d_samples_per_code; j++) { - complex_acumulator[i] += (corr_output[j]); + complex_acumulator[i] += (d_corr_output[j]); } } // Obtain maximum value of correlation given the possible delay selected diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h index f2ca4759c..32b54a9c8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h @@ -183,8 +183,10 @@ private: std::vector> d_grid_doppler_wipeoffs; std::vector d_code; std::vector d_fft_codes; - std::vector d_signal_folded; std::vector d_code_folded; + std::vector d_in_temp; + std::vector d_in_1code; + std::vector d_corr_output; std::vector d_magnitude; std::vector d_corr_output_f; std::vector d_magnitude_folded; From bdfc7883fdf94931ab2fe7387f7fe13d4135b731 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 30 Jun 2026 20:04:51 +0200 Subject: [PATCH 2/3] Acquisition: reuse PCPS assisted and fine Doppler buffers --- .../pcps_acquisition_fine_doppler_cc.cc | 89 +++++++++---------- .../pcps_acquisition_fine_doppler_cc.h | 9 ++ .../pcps_assisted_acquisition_cc.cc | 16 ++-- .../pcps_assisted_acquisition_cc.h | 3 +- 4 files changed, 60 insertions(+), 57 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index c2aef9a46..f90d5e542 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -57,6 +57,10 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_well_count(0), d_n_samples_in_buffer(0), d_fft_size(static_cast(conf_.samples_per_ms)), + d_fine_doppler_zero_padding_factor(8), + d_fine_doppler_prn_replicas(10), + d_fine_doppler_signal_samples(d_fine_doppler_prn_replicas * d_fft_size), + d_fine_doppler_fft_size(d_fine_doppler_signal_samples * d_fine_doppler_zero_padding_factor), d_gnuradio_forecast_samples(d_fft_size), d_channel(0), d_dump_channel(0), @@ -64,14 +68,19 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_dump(conf_.dump), d_fft_if(gnss_fft_fwd_make_unique(d_fft_size)), d_ifft(gnss_fft_rev_make_unique(d_fft_size)), + d_fine_doppler_fft(gnss_fft_fwd_make_unique(d_fine_doppler_fft_size)), d_grid_data(d_num_doppler_points, volk_gnsssdr::vector(d_fft_size)), d_fft_codes(d_fft_size), d_10_ms_buffer(50 * d_fft_size), - d_magnitude(d_fft_size) + d_magnitude(d_fft_size), + d_fine_doppler_code_replica(d_fine_doppler_signal_samples), + d_fine_doppler_magnitude(d_fine_doppler_fft_size), + d_fine_doppler_freq_bins(d_fine_doppler_fft_size) { this->message_port_register_out(pmt::mp("events")); update_carrier_wipeoff(); + update_fine_doppler_freq_bins(); // this implementation can only produce dumps in channel 0 // todo: migrate config parameters to the unified acquisition config class @@ -179,6 +188,23 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() } +void pcps_acquisition_fine_doppler_cc::update_fine_doppler_freq_bins() +{ + int counter = 0; + for (int k = 0; k < (d_fine_doppler_fft_size / 2); k++) + { + d_fine_doppler_freq_bins[counter] = ((static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(d_fine_doppler_fft_size) / 2.0); + counter++; + } + + for (int k = d_fine_doppler_fft_size / 2; k > 0; k--) + { + d_fine_doppler_freq_bins[counter] = ((-static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(d_fine_doppler_fft_size) / 2.0); + counter++; + } +} + + float pcps_acquisition_fine_doppler_cc::compute_CAF() { float firstPeak = 0.0; @@ -275,9 +301,6 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons << ", doppler_max: " << d_acq_params.doppler_max << ", doppler_step: " << d_acq_params.doppler_step; - // 2- Doppler frequency search loop - volk_gnsssdr::vector p_tmp_vector(d_fft_size); - for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { // doppler search steps @@ -296,9 +319,9 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons d_ifft->execute(); // save the grid matrix delay file - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), d_ifft->get_outbuf(), d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size); // accumulate grid values - volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_grid_data[doppler_index].data(), p_tmp_vector.data(), d_fft_size); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_grid_data[doppler_index].data(), d_magnitude.data(), d_fft_size); } return d_fft_size; @@ -315,73 +338,45 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons int pcps_acquisition_fine_doppler_cc::estimate_Doppler() { - // Direct FFT - int zero_padding_factor = 8; - int prn_replicas = 10; - int signal_samples = prn_replicas * d_fft_size; - // int fft_size_extended = nextPowerOf2(signal_samples * zero_padding_factor); - int fft_size_extended = signal_samples * zero_padding_factor; - - auto fft_operator = gnss_fft_fwd_make_unique(fft_size_extended); - // zero padding the entire vector - std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0)); + std::fill_n(d_fine_doppler_fft->get_inbuf(), d_fine_doppler_fft_size, gr_complex(0.0, 0.0)); // 1. generate local code aligned with the acquisition code phase estimation - volk_gnsssdr::vector code_replica(signal_samples); - - gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_acq_params.fs_in, 0); + gps_l1_ca_code_gen_complex_sampled(d_fine_doppler_code_replica, d_gnss_synchro->PRN, d_acq_params.fs_in, 0); int shift_index = static_cast(d_gnss_synchro->Acq_delay_samples); // Rotate to align the local code replica using acquisition time delay estimation if (shift_index != 0) { - std::rotate(code_replica.data(), code_replica.data() + (d_fft_size - shift_index), code_replica.data() + d_fft_size - 1); + std::rotate(d_fine_doppler_code_replica.data(), d_fine_doppler_code_replica.data() + (d_fft_size - shift_index), d_fine_doppler_code_replica.data() + d_fft_size - 1); } - for (int n = 0; n < prn_replicas - 1; n++) + for (int n = 0; n < d_fine_doppler_prn_replicas - 1; n++) { - std::copy_n(code_replica.data(), d_fft_size, &code_replica[(n + 1) * d_fft_size]); + std::copy_n(d_fine_doppler_code_replica.data(), d_fft_size, &d_fine_doppler_code_replica[(n + 1) * d_fft_size]); } // 2. Perform code wipe-off - volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), d_10_ms_buffer.data(), code_replica.data(), signal_samples); + volk_32fc_x2_multiply_32fc(d_fine_doppler_fft->get_inbuf(), d_10_ms_buffer.data(), d_fine_doppler_code_replica.data(), d_fine_doppler_signal_samples); // 3. Perform the FFT (zero padded!) - fft_operator->execute(); + d_fine_doppler_fft->execute(); // 4. Compute the magnitude and find the maximum - volk_gnsssdr::vector p_tmp_vector(fft_size_extended); - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), fft_operator->get_outbuf(), fft_size_extended); + volk_32fc_magnitude_squared_32f(d_fine_doppler_magnitude.data(), d_fine_doppler_fft->get_outbuf(), d_fine_doppler_fft_size); uint32_t tmp_index_freq = 0; - volk_gnsssdr_32f_index_max_32u(&tmp_index_freq, p_tmp_vector.data(), fft_size_extended); - - // case even - int counter = 0; - volk_gnsssdr::vector fftFreqBins(fft_size_extended); - - for (int k = 0; k < (fft_size_extended / 2); k++) - { - fftFreqBins[counter] = ((static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(fft_size_extended) / 2.0); - counter++; - } - - for (int k = fft_size_extended / 2; k > 0; k--) - { - fftFreqBins[counter] = ((-static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(fft_size_extended) / 2.0); - counter++; - } + volk_gnsssdr_32f_index_max_32u(&tmp_index_freq, d_fine_doppler_magnitude.data(), d_fine_doppler_fft_size); // 5. Update the Doppler estimation in Hz - if (std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000) + if (std::abs(d_fine_doppler_freq_bins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000) { - d_gnss_synchro->Acq_doppler_hz = static_cast(fftFreqBins[tmp_index_freq]); - // std::cout << "FFT maximum present at " << fftFreqBins[tmp_index_freq] << " [Hz]\n"; + d_gnss_synchro->Acq_doppler_hz = static_cast(d_fine_doppler_freq_bins[tmp_index_freq]); + // std::cout << "FFT maximum present at " << d_fine_doppler_freq_bins[tmp_index_freq] << " [Hz]\n"; } else { - DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz); + DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(d_fine_doppler_freq_bins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz); DLOG(INFO) << "Error estimating fine frequency Doppler"; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h index 97e471bdc..09c26c004 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h @@ -163,6 +163,7 @@ private: float compute_CAF(); void reset_grid(); void update_carrier_wipeoff(); + void update_fine_doppler_freq_bins(); bool start() override; arma::fmat grid_; @@ -185,6 +186,10 @@ private: int d_well_count; int d_n_samples_in_buffer; const int d_fft_size; + const int d_fine_doppler_zero_padding_factor; + const int d_fine_doppler_prn_replicas; + const int d_fine_doppler_signal_samples; + const int d_fine_doppler_fft_size; int d_gnuradio_forecast_samples; unsigned int d_channel; unsigned int d_dump_channel; @@ -195,12 +200,16 @@ private: std::weak_ptr d_channel_fsm; std::unique_ptr d_fft_if; std::unique_ptr d_ifft; + std::unique_ptr d_fine_doppler_fft; volk_gnsssdr::vector>> d_grid_doppler_wipeoffs; volk_gnsssdr::vector> d_grid_data; volk_gnsssdr::vector d_fft_codes; volk_gnsssdr::vector d_10_ms_buffer; volk_gnsssdr::vector d_magnitude; + volk_gnsssdr::vector d_fine_doppler_code_replica; + volk_gnsssdr::vector d_fine_doppler_magnitude; + volk_gnsssdr::vector d_fine_doppler_freq_bins; }; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index bbac7bf02..28fc7f586 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -66,7 +66,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(const Acq_Conf &conf) d_disable_assist(false), d_fft_if(gnss_fft_fwd_make_unique(d_fft_size)), d_ifft(gnss_fft_rev_make_unique(d_fft_size)), - d_fft_codes(d_fft_size) + d_fft_codes(d_fft_size), + d_magnitude(d_fft_size) { this->message_port_register_out(pmt::mp("events")); } @@ -231,16 +232,15 @@ float pcps_assisted_acquisition_cc::search_maximum() } -float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) const +float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) { const auto *in = reinterpret_cast(input_items[0]); // Get the input samples pointer // 1- Compute the input signal power estimation - std::vector p_tmp_vector(d_fft_size); - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), in, d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size); float power; - volk_32f_accumulator_s32f(&power, p_tmp_vector.data(), d_fft_size); + volk_32f_accumulator_s32f(&power, d_magnitude.data(), d_fft_size); return (power / static_cast(d_fft_size)); } @@ -258,8 +258,6 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons << ", doppler_step: " << d_acq_params.doppler_step; // 2- Doppler frequency search loop - std::vector p_tmp_vector(d_fft_size); - for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { // doppler search steps @@ -277,9 +275,9 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons d_ifft->execute(); // save the grid matrix delay file - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), d_ifft->get_outbuf(), d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size); const float *old_vector = d_grid_data[doppler_index].data(); - volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector.data(), d_fft_size); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, d_magnitude.data(), d_fft_size); } return d_fft_size; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h index 6a0a1f557..55faced3b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h @@ -148,7 +148,7 @@ private: void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, int32_t doppler_offset); int32_t compute_and_accumulate_grid(gr_vector_const_void_star& input_items); - float estimate_input_power(gr_vector_const_void_star& input_items) const; + float estimate_input_power(gr_vector_const_void_star& input_items); float search_maximum(); void get_assistance(); void reset_grid(); @@ -187,6 +187,7 @@ private: std::vector>> d_grid_doppler_wipeoffs; std::vector> d_grid_data; std::vector d_fft_codes; + std::vector d_magnitude; }; From 3bfbb1904d33f844602a2ea84a3c1b442c662e34 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 30 Jun 2026 20:29:50 +0200 Subject: [PATCH 3/3] Prepare PCPS grids for batch FFT processing Store PCPS Doppler wipeoffs and magnitude grids in flat, stride-aligned buffers instead of nested vectors. This reduces per-row indirection, improves cache locality, and makes the Doppler-bin data layout suitable for future batch FFT planning. --- .../gnuradio_blocks/pcps_acquisition.cc | 211 ++++++++++++------ .../gnuradio_blocks/pcps_acquisition.h | 15 +- 2 files changed, 158 insertions(+), 68 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 18917aaca..5b7faa390 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -90,6 +90,23 @@ std::string get_dump_filename(std::string dump_filename) return dump_path + fs::path::preferred_separator + dump_filename; } + +template +uint32_t aligned_row_stride(uint32_t row_size) +{ + const auto alignment = static_cast(volk_get_alignment()); + if (alignment == 0) + { + return row_size; + } + + uint32_t stride = row_size; + while ((static_cast(stride) * sizeof(T)) % alignment != 0) + { + ++stride; + } + return stride; +} } // namespace pcps_acquisition_sptr pcps_make_acquisition(const Acq_Conf& conf_) @@ -110,6 +127,8 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) d_consumed_samples(conf_.sampled_ms * conf_.samples_per_ms * (conf_.bit_transition_flag ? 2.0 : 1.0)), d_fft_size(conf_.sampled_ms == conf_.ms_per_code ? d_consumed_samples : d_consumed_samples * 2), d_effective_fft_size(conf_.bit_transition_flag ? (d_fft_size / 2) : d_fft_size), + d_magnitude_grid_stride(aligned_row_stride(d_effective_fft_size)), + d_doppler_wipeoffs_stride(aligned_row_stride(d_fft_size)), d_num_doppler_bins(static_cast(std::ceil(static_cast(2 * d_doppler_max) / static_cast(d_doppler_step)))), d_num_doppler_bins_step2(conf_.num_doppler_bins_step2), d_dump_channel(conf_.dump_channel), @@ -134,11 +153,12 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) d_dump_number(0), d_input_power(0), d_doppler_center_step_two(0), - d_magnitude_grid(d_num_doppler_bins, volk_gnsssdr::vector(d_fft_size)), + d_magnitude_grid(std::max(d_num_doppler_bins, d_num_doppler_bins_step2) * d_magnitude_grid_stride), d_tmp_buffer(d_effective_fft_size), d_input_signal(d_fft_size), + d_grid_doppler_wipeoffs_step_two(d_acq_parameters.make_2_steps ? d_num_doppler_bins_step2 * d_doppler_wipeoffs_stride : 0), d_ifft(gnss_fft_rev_make_unique(d_fft_size)), - d_grid_doppler_wipeoffs(d_num_doppler_bins, volk_gnsssdr::vector>(d_fft_size)), + d_grid_doppler_wipeoffs(d_num_doppler_bins * d_doppler_wipeoffs_stride), d_fft_codes(d_fft_size), d_data_buffer(d_consumed_samples), d_fft_if(gnss_fft_fwd_make_unique(d_fft_size)) @@ -168,23 +188,9 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) d_data_buffer_sc = volk_gnsssdr::vector(d_consumed_samples); } - if (d_acq_parameters.make_2_steps) - { - d_grid_doppler_wipeoffs_step_two = volk_gnsssdr::vector>>(d_num_doppler_bins_step2, volk_gnsssdr::vector>(d_fft_size)); - } - - for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) - { - std::fill(d_magnitude_grid[doppler_index].begin(), d_magnitude_grid[doppler_index].end(), 0.0); - } + std::fill(d_magnitude_grid.begin(), d_magnitude_grid.end(), 0.0F); update_grid_doppler_wipeoffs(); - - if (d_dump) - { - d_grid = arma::fmat(d_effective_fft_size, d_num_doppler_bins, arma::fill::zeros); - d_narrow_grid = arma::fmat(d_effective_fft_size, d_num_doppler_bins_step2, arma::fill::zeros); - } } @@ -286,7 +292,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs() for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) { const int32_t doppler = -static_cast(d_doppler_max) + d_doppler_center + d_doppler_step * doppler_index; - update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], static_cast(d_doppler_bias + doppler)); + update_local_carrier(own::span(doppler_wipeoff_data(doppler_index), d_fft_size), static_cast(d_doppler_bias + doppler)); } } @@ -298,7 +304,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2() const float doppler = (static_cast(doppler_index) - static_cast(floor(d_num_doppler_bins_step2 / 2.0))) * d_acq_parameters.doppler_step2; // d_doppler_center_step_two holds the unbiased Doppler reported by the coarse // stage, so the FDMA frequency offset must be added back to the wipeoff - update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], static_cast(d_doppler_bias) + d_doppler_center_step_two + doppler); + update_local_carrier(own::span(doppler_wipeoff_step_two_data(doppler_index), d_fft_size), static_cast(d_doppler_bias) + d_doppler_center_step_two + doppler); } } @@ -355,6 +361,8 @@ void pcps_acquisition::send_negative_acquisition(const AcquisitionResult& result void pcps_acquisition::dump_results(const AcquisitionResult& result) { + ensure_dump_grid_allocated(); + d_dump_number++; std::string filename = d_dump_filename; filename.append("_"); @@ -408,6 +416,66 @@ void pcps_acquisition::dump_results(const AcquisitionResult& result) } +void pcps_acquisition::ensure_dump_grid_allocated() +{ + if ((d_grid.n_rows != d_effective_fft_size) || (d_grid.n_cols != d_num_doppler_bins)) + { + d_grid.zeros(d_effective_fft_size, d_num_doppler_bins); + } + + if (d_acq_parameters.make_2_steps && + ((d_narrow_grid.n_rows != d_effective_fft_size) || (d_narrow_grid.n_cols != d_num_doppler_bins_step2))) + { + d_narrow_grid.zeros(d_effective_fft_size, d_num_doppler_bins_step2); + } +} + + +void pcps_acquisition::copy_magnitude_grid_to_dump_grid() +{ + ensure_dump_grid_allocated(); + + const auto bin_count = d_step_two ? d_num_doppler_bins_step2 : d_num_doppler_bins; + auto& grid = d_step_two ? d_narrow_grid : d_grid; + + for (uint32_t doppler_index = 0; doppler_index < bin_count; doppler_index++) + { + const auto* magnitude_grid = magnitude_grid_data(doppler_index); + std::copy(magnitude_grid, magnitude_grid + d_effective_fft_size, grid.colptr(doppler_index)); + } +} + + +bool pcps_acquisition::should_dump_channel() const +{ + return d_dump && d_channel == d_dump_channel; +} + + +gr_complex* pcps_acquisition::doppler_wipeoff_data(uint32_t doppler_index) +{ + return d_grid_doppler_wipeoffs.data() + static_cast(doppler_index) * d_doppler_wipeoffs_stride; +} + + +gr_complex* pcps_acquisition::doppler_wipeoff_step_two_data(uint32_t doppler_index) +{ + return d_grid_doppler_wipeoffs_step_two.data() + static_cast(doppler_index) * d_doppler_wipeoffs_stride; +} + + +float* pcps_acquisition::magnitude_grid_data(uint32_t doppler_index) +{ + return d_magnitude_grid.data() + static_cast(doppler_index) * d_magnitude_grid_stride; +} + + +const float* pcps_acquisition::magnitude_grid_data(uint32_t doppler_index) const +{ + return d_magnitude_grid.data() + static_cast(doppler_index) * d_magnitude_grid_stride; +} + + pcps_acquisition::AcquisitionResult pcps_acquisition::max_to_input_power_statistic(uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step) { AcquisitionResult result; @@ -418,10 +486,11 @@ pcps_acquisition::AcquisitionResult pcps_acquisition::max_to_input_power_statist // Find the correlation peak and the carrier frequency for (uint32_t i = 0; i < num_doppler_bins; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i].data(), d_effective_fft_size); - if (d_magnitude_grid[i][tmp_intex_t] > grid_maximum) + const auto* magnitude_grid = magnitude_grid_data(i); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, magnitude_grid, d_effective_fft_size); + if (magnitude_grid[tmp_intex_t] > grid_maximum) { - grid_maximum = d_magnitude_grid[i][tmp_intex_t]; + grid_maximum = magnitude_grid[tmp_intex_t]; index_doppler = i; result.index_time = tmp_intex_t; } @@ -430,7 +499,8 @@ pcps_acquisition::AcquisitionResult pcps_acquisition::max_to_input_power_statist if (!d_step_two) { const auto index_opp = (index_doppler + d_num_doppler_bins / 2) % d_num_doppler_bins; - d_input_power = static_cast(std::accumulate(d_magnitude_grid[index_opp].data(), d_magnitude_grid[index_opp].data() + d_effective_fft_size, static_cast(0.0)) / d_effective_fft_size / 2.0 / d_num_noncoherent_integrations_counter); + const auto* magnitude_grid = magnitude_grid_data(index_opp); + d_input_power = static_cast(std::accumulate(magnitude_grid, magnitude_grid + d_effective_fft_size, static_cast(0.0)) / d_effective_fft_size / 2.0 / d_num_noncoherent_integrations_counter); result.doppler = -static_cast(doppler_max) + d_doppler_center + doppler_step * static_cast(index_doppler); } else @@ -465,10 +535,11 @@ pcps_acquisition::AcquisitionResult pcps_acquisition::first_vs_second_peak_stati // Find the correlation peak and the carrier frequency for (uint32_t i = 0; i < num_doppler_bins; i++) { - volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i].data(), d_effective_fft_size); - if (d_magnitude_grid[i][tmp_intex_t] > firstPeak) + const auto* magnitude_grid = magnitude_grid_data(i); + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, magnitude_grid, d_effective_fft_size); + if (magnitude_grid[tmp_intex_t] > firstPeak) { - firstPeak = d_magnitude_grid[i][tmp_intex_t]; + firstPeak = magnitude_grid[tmp_intex_t]; index_doppler = i; result.index_time = tmp_intex_t; } @@ -498,7 +569,8 @@ pcps_acquisition::AcquisitionResult pcps_acquisition::first_vs_second_peak_stati } int32_t idx = excludeRangeIndex1; - std::copy(d_magnitude_grid[index_doppler].data(), d_magnitude_grid[index_doppler].data() + d_effective_fft_size, d_tmp_buffer.data()); + const auto* magnitude_grid = magnitude_grid_data(index_doppler); + std::copy(magnitude_grid, magnitude_grid + d_effective_fft_size, d_tmp_buffer.data()); do { d_tmp_buffer[idx] = 0.0; @@ -524,13 +596,15 @@ pcps_acquisition::AcquisitionResult pcps_acquisition::first_vs_second_peak_stati void pcps_acquisition::doppler_grid(const gr_complex* in) { const auto bin_count = d_step_two ? d_num_doppler_bins_step2 : d_num_doppler_bins; - const auto& grid_doppler_wipeoffs = d_step_two ? d_grid_doppler_wipeoffs_step_two : d_grid_doppler_wipeoffs; - auto& grid = d_step_two ? d_narrow_grid : d_grid; + const auto* grid_doppler_wipeoffs = d_step_two ? d_grid_doppler_wipeoffs_step_two.data() : d_grid_doppler_wipeoffs.data(); for (uint32_t doppler_index = 0; doppler_index < bin_count; doppler_index++) { + auto* magnitude_grid = magnitude_grid_data(doppler_index); + const auto* doppler_wipeoff = grid_doppler_wipeoffs + static_cast(doppler_index) * d_doppler_wipeoffs_stride; + // Remove Doppler - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, grid_doppler_wipeoffs[doppler_index].data(), d_fft_size); + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, doppler_wipeoff, d_fft_size); // Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal @@ -546,17 +620,12 @@ void pcps_acquisition::doppler_grid(const gr_complex* in) const size_t offset = (d_acq_parameters.bit_transition_flag ? d_effective_fft_size : 0); if (d_num_noncoherent_integrations_counter == 1) { - volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, d_effective_fft_size); + volk_32fc_magnitude_squared_32f(magnitude_grid, d_ifft->get_outbuf() + offset, d_effective_fft_size); } else { volk_32fc_magnitude_squared_32f(d_tmp_buffer.data(), d_ifft->get_outbuf() + offset, d_effective_fft_size); - volk_32f_x2_add_32f(d_magnitude_grid[doppler_index].data(), d_magnitude_grid[doppler_index].data(), d_tmp_buffer.data(), d_effective_fft_size); - } - // Record results to file if required - if (d_dump && d_channel == d_dump_channel) - { - std::copy(d_magnitude_grid[doppler_index].data(), d_magnitude_grid[doppler_index].data() + d_effective_fft_size, grid.colptr(doppler_index)); + volk_32f_x2_add_32f(magnitude_grid, magnitude_grid, d_tmp_buffer.data(), d_effective_fft_size); } } } @@ -652,19 +721,26 @@ void pcps_acquisition::acquisition_core(uint64_t sample_count) gr::thread::scoped_lock lk(d_setlock); // Initialize acquisition algorithm + const gr_complex* in = nullptr; // Get the input samples pointer if (d_cshort) { - volk_gnsssdr_16ic_convert_32fc(d_data_buffer.data(), d_data_buffer_sc.data(), d_consumed_samples); - } - std::copy(d_data_buffer.data(), d_data_buffer.data() + d_consumed_samples, d_input_signal.data()); - if (d_fft_size > d_consumed_samples) - { - for (uint32_t i = d_consumed_samples; i < d_fft_size; i++) + volk_gnsssdr_16ic_convert_32fc(d_input_signal.data(), d_data_buffer_sc.data(), d_consumed_samples); + if (d_fft_size > d_consumed_samples) { - d_input_signal[i] = gr_complex(0.0, 0.0); + std::fill_n(d_input_signal.data() + d_consumed_samples, d_fft_size - d_consumed_samples, gr_complex(0.0, 0.0)); } + in = d_input_signal.data(); + } + else if (d_fft_size == d_consumed_samples) + { + in = d_data_buffer.data(); + } + else + { + std::copy(d_data_buffer.data(), d_data_buffer.data() + d_consumed_samples, d_input_signal.data()); + std::fill_n(d_input_signal.data() + d_consumed_samples, d_fft_size - d_consumed_samples, gr_complex(0.0, 0.0)); + in = d_input_signal.data(); } - const gr_complex* in = d_input_signal.data(); // Get the input samples pointer d_num_noncoherent_integrations_counter++; @@ -680,6 +756,10 @@ void pcps_acquisition::acquisition_core(uint64_t sample_count) // Doppler frequency grid loop, only access variables that doesn't need a lock doppler_grid(in); + if (should_dump_channel()) + { + copy_magnitude_grid_to_dump_grid(); + } auto result = compute_statistics(); result.sample_count = sample_count; @@ -719,7 +799,7 @@ void pcps_acquisition::acquisition_core(uint64_t sample_count) if ((d_num_noncoherent_integrations_counter == d_acq_parameters.max_dwells) || (result.positive_acq) || (d_acq_parameters.bit_transition_flag)) { // Record results to file if required - if (d_dump && d_channel == d_dump_channel) + if (should_dump_channel()) { pcps_acquisition::dump_results(result); } @@ -748,6 +828,26 @@ void pcps_acquisition::set_doppler_center(int32_t doppler_center) } +void pcps_acquisition::wait_if_active() +{ + std::unique_ptr worker; + + { + gr::thread::scoped_lock lk(d_setlock); + worker = std::move(d_worker); + d_worker = nullptr; + } + + if (worker != nullptr) + { + if (worker->joinable()) + { + worker->join(); + } + } +} + + int pcps_acquisition::general_work(int noutput_items __attribute__((unused)), gr_vector_int& ninput_items, gr_vector_const_void_star& input_items, @@ -853,22 +953,3 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)), return 0; } - -void pcps_acquisition::wait_if_active() -{ - std::unique_ptr worker; - - { - gr::thread::scoped_lock lk(d_setlock); - worker = std::move(d_worker); - d_worker = nullptr; - } - - if (worker != nullptr) - { - if (worker->joinable()) - { - worker->join(); - } - } -} diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index a9229fd19..b07578c08 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -185,6 +185,13 @@ private: void send_negative_acquisition(const AcquisitionResult& result); void send_positive_acquisition(const AcquisitionResult& result); void dump_results(const AcquisitionResult& result); + void ensure_dump_grid_allocated(); + void copy_magnitude_grid_to_dump_grid(); + bool should_dump_channel() const; + std::complex* doppler_wipeoff_data(uint32_t doppler_index); + std::complex* doppler_wipeoff_step_two_data(uint32_t doppler_index); + float* magnitude_grid_data(uint32_t doppler_index); + const float* magnitude_grid_data(uint32_t doppler_index) const; bool is_fdma(); float get_threshold() const; AcquisitionResult first_vs_second_peak_statistic(uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step); @@ -199,6 +206,8 @@ private: const uint32_t d_consumed_samples; const uint32_t d_fft_size; const uint32_t d_effective_fft_size; + const uint32_t d_magnitude_grid_stride; + const uint32_t d_doppler_wipeoffs_stride; const uint32_t d_num_doppler_bins; const uint32_t d_num_doppler_bins_step2; const uint32_t d_dump_channel; @@ -229,16 +238,16 @@ private: int64_t d_dump_number; float d_input_power; float d_doppler_center_step_two; - volk_gnsssdr::vector> d_magnitude_grid; + volk_gnsssdr::vector d_magnitude_grid; volk_gnsssdr::vector d_tmp_buffer; volk_gnsssdr::vector> d_input_signal; - volk_gnsssdr::vector>> d_grid_doppler_wipeoffs_step_two; + volk_gnsssdr::vector> d_grid_doppler_wipeoffs_step_two; std::unique_ptr d_ifft; arma::fmat d_grid; arma::fmat d_narrow_grid; // These are never accessed outside acquisition_core while acquisition is active - volk_gnsssdr::vector>> d_grid_doppler_wipeoffs; + volk_gnsssdr::vector> d_grid_doppler_wipeoffs; volk_gnsssdr::vector> d_fft_codes; volk_gnsssdr::vector> d_data_buffer; volk_gnsssdr::vector d_data_buffer_sc;