mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-18 13:32:37 +00:00
Code Issues Releases Wiki Activity

Merge branch 'better-acq' into next

Browse Source
This commit is contained in:
Carles Fernandez
2026-07-05 13:47:44 +02:00
parent 8f757b8c21 3bfbb1904d
commit b3c4dbccaf
8 changed files with 232 additions and 147 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;
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
+42 -47
View File
@@ -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<int32_t>(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<float>(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<float>(d_acq_params.fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(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<float>(d_acq_params.fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(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<float> 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<gr_complex> 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<int>(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<float> 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<float> fftFreqBins(fft_size_extended);
for (int k = 0; k < (fft_size_extended / 2); k++)
{
fftFreqBins[counter] = ((static_cast<float>(d_acq_params.fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
counter++;
}
for (int k = fft_size_extended / 2; k > 0; k--)
{
fftFreqBins[counter] = ((-static_cast<float>(d_acq_params.fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(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<double>(fftFreqBins[tmp_index_freq]);
// std::cout << "FFT maximum present at " << fftFreqBins[tmp_index_freq] << " [Hz]\n";
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(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";
}
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h
+9
View File
@@ -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<ChannelFsm> d_channel_fsm;
std::unique_ptr<gnss_fft_complex_fwd> d_fft_if;
std::unique_ptr<gnss_fft_complex_rev> d_ifft;
std::unique_ptr<gnss_fft_complex_fwd> d_fine_doppler_fft;
volk_gnsssdr::vector<volk_gnsssdr::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
volk_gnsssdr::vector<volk_gnsssdr::vector<float>> d_grid_data;
volk_gnsssdr::vector<gr_complex> d_fft_codes;
volk_gnsssdr::vector<gr_complex> d_10_ms_buffer;
volk_gnsssdr::vector<float> d_magnitude;
volk_gnsssdr::vector<gr_complex> d_fine_doppler_code_replica;
volk_gnsssdr::vector<float> d_fine_doppler_magnitude;
volk_gnsssdr::vector<float> d_fine_doppler_freq_bins;
};
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
+7 -9
View File
@@ -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<const gr_complex *>(input_items[0]); // Get the input samples pointer
// 1- Compute the input signal power estimation
std::vector<float> 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<float>(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<float> 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;
}
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h
+2 -1
View File
@@ -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<std::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
std::vector<std::vector<float>> d_grid_data;
std::vector<gr_complex> d_fft_codes;
std::vector<float> d_magnitude;
};
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
+11 -21
View File
@@ -19,6 +19,7 @@
#include <gnuradio/io_signature.h>
#include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <algorithm>
#include <array>
#include <cmath>
#include <exception>
@@ -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<const gr_complex*>(input_items[0]); // Get the input samples pointer
std::vector<gr_complex> 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<gr_complex> 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<gr_complex> 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<float>(d_fft_size) * static_cast<float>(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<gr_complex>(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<int32_t>(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<gr_complex>());
@@ -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
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h
+3 -1
View File
@@ -183,8 +183,10 @@ private:
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
std::vector<gr_complex> d_code;
std::vector<gr_complex> d_fft_codes;
std::vector<gr_complex> d_signal_folded;
std::vector<gr_complex> d_code_folded;
std::vector<gr_complex> d_in_temp;
std::vector<gr_complex> d_in_1code;
std::vector<gr_complex> d_corr_output;
std::vector<float> d_magnitude;
std::vector<float> d_corr_output_f;
std::vector<float> d_magnitude_folded;