From 3bfbb1904d33f844602a2ea84a3c1b442c662e34 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 30 Jun 2026 20:29:50 +0200 Subject: [PATCH] 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;