mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-14 03:31:50 +00:00
Code Issues Releases Wiki Activity

Prepare PCPS grids for batch FFT processing

Browse Source 
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.
This commit is contained in:
Carles Fernandez
2026-06-30 20:29:50 +02:00
parent bdfc7883fd
commit 3bfbb1904d
2 changed files with 158 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
+146 -65
View File
@@ -90,6 +90,23 @@ std::string get_dump_filename(std::string dump_filename)
return dump_path + fs::path::preferred_separator + dump_filename;
}
template <typename T>
uint32_t aligned_row_stride(uint32_t row_size)
{
const auto alignment = static_cast<size_t>(volk_get_alignment());
if (alignment == 0)
{
return row_size;
}
uint32_t stride = row_size;
while ((static_cast<size_t>(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<float>(d_effective_fft_size)),
d_doppler_wipeoffs_stride(aligned_row_stride<gr_complex>(d_fft_size)),
d_num_doppler_bins(static_cast<uint32_t>(std::ceil(static_cast<double>(2 * d_doppler_max) / static_cast<double>(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<float>(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<std::complex<float>>(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<lv_16sc_t>(d_consumed_samples);
}
if (d_acq_parameters.make_2_steps)
{
d_grid_doppler_wipeoffs_step_two = volk_gnsssdr::vector<volk_gnsssdr::vector<std::complex<float>>>(d_num_doppler_bins_step2, volk_gnsssdr::vector<std::complex<float>>(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<int32_t>(d_doppler_max) + d_doppler_center + d_doppler_step * doppler_index;
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], static_cast<float>(d_doppler_bias + doppler));
update_local_carrier(own::span<gr_complex>(doppler_wipeoff_data(doppler_index), d_fft_size), static_cast<float>(d_doppler_bias + doppler));
}
}
@@ -298,7 +304,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2()
const float doppler = (static_cast<float>(doppler_index) - static_cast<float>(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<float>(d_doppler_bias) + d_doppler_center_step_two + doppler);
update_local_carrier(own::span<gr_complex>(doppler_wipeoff_step_two_data(doppler_index), d_fft_size), static_cast<float>(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<size_t>(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<size_t>(doppler_index) * d_doppler_wipeoffs_stride;
}
float* pcps_acquisition::magnitude_grid_data(uint32_t doppler_index)
{
return d_magnitude_grid.data() + static_cast<size_t>(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<size_t>(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<float>(std::accumulate(d_magnitude_grid[index_opp].data(), d_magnitude_grid[index_opp].data() + d_effective_fft_size, static_cast<float>(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<float>(std::accumulate(magnitude_grid, magnitude_grid + d_effective_fft_size, static_cast<float>(0.0)) / d_effective_fft_size / 2.0 / d_num_noncoherent_integrations_counter);
result.doppler = -static_cast<int32_t>(doppler_max) + d_doppler_center + doppler_step * static_cast<int32_t>(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<size_t>(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<gr::thread::thread> 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<gr::thread::thread> worker;
{
gr::thread::scoped_lock lk(d_setlock);
worker = std::move(d_worker);
d_worker = nullptr;
}
if (worker != nullptr)
{
if (worker->joinable())
{
worker->join();
}
}
}
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h
+12 -3
View File
@@ -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<float>* doppler_wipeoff_data(uint32_t doppler_index);
std::complex<float>* 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<volk_gnsssdr::vector<float>> d_magnitude_grid;
volk_gnsssdr::vector<float> d_magnitude_grid;
volk_gnsssdr::vector<float> d_tmp_buffer;
volk_gnsssdr::vector<std::complex<float>> d_input_signal;
volk_gnsssdr::vector<volk_gnsssdr::vector<std::complex<float>>> d_grid_doppler_wipeoffs_step_two;
volk_gnsssdr::vector<std::complex<float>> d_grid_doppler_wipeoffs_step_two;
std::unique_ptr<gnss_fft_complex_rev> 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<volk_gnsssdr::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
volk_gnsssdr::vector<std::complex<float>> d_grid_doppler_wipeoffs;
volk_gnsssdr::vector<std::complex<float>> d_fft_codes;
volk_gnsssdr::vector<std::complex<float>> d_data_buffer;
volk_gnsssdr::vector<lv_16sc_t> d_data_buffer_sc;