mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-24 05:53:16 +00:00
Code Issues Releases Wiki Activity

Switch to automatically managed static memory

Browse Source
This commit is contained in:
Carles Fernandez 2019-07-23 18:54:32 +02:00
parent 31b6f9defd
commit 05faf97b70
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
14 changed files with 278 additions and 499 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

188
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

@ -106,42 +106,26 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
d_both_signal_components = both_signal_components_;
d_CAF_window_hz = CAF_window_hz_;
d_inbuffer = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_code_I_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitudeIA = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_inbuffer.reserve(d_fft_size);
d_fft_code_I_A.reserve(d_fft_size);
d_magnitudeIA.reserve(d_fft_size);
if (d_both_signal_components == true)
{
d_fft_code_Q_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitudeQA = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
}
else
{
d_fft_code_Q_A = nullptr;
d_magnitudeQA = nullptr;
d_fft_code_Q_A.reserve(d_fft_size);
d_magnitudeQA.reserve(d_fft_size);
}
// IF COHERENT INTEGRATION TIME > 1
if (d_sampled_ms > 1)
{
d_fft_code_I_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitudeIB = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_code_I_B.reserve(d_fft_size);
d_magnitudeIB.reserve(d_fft_size);
if (d_both_signal_components == true)
{
d_fft_code_Q_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitudeQB = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_code_Q_B.reserve(d_fft_size);
d_magnitudeQB.reserve(d_fft_size);
}
else
{
d_fft_code_Q_B = nullptr;
d_magnitudeQB = nullptr;
}
}
else
{
d_fft_code_I_B = nullptr;
d_magnitudeIB = nullptr;
d_fft_code_Q_B = nullptr;
d_magnitudeQB = nullptr;
}
// Direct FFT
@ -157,14 +141,10 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 250;
d_grid_doppler_wipeoffs = nullptr;
d_gnss_synchro = nullptr;
d_code_phase = 0;
d_doppler_freq = 0;
d_test_statistics = 0;
d_CAF_vector = nullptr;
d_CAF_vector_I = nullptr;
d_CAF_vector_Q = nullptr;
d_channel = 0;
d_gr_stream_buffer = 0;
}
@ -172,44 +152,6 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisition_caf_cc()
{
if (d_num_doppler_bins > 0)
{
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
volk_gnsssdr_free(d_inbuffer);
volk_gnsssdr_free(d_fft_code_I_A);
volk_gnsssdr_free(d_magnitudeIA);
if (d_both_signal_components == true)
{
volk_gnsssdr_free(d_fft_code_Q_A);
volk_gnsssdr_free(d_magnitudeQA);
}
// IF INTEGRATION TIME > 1
if (d_sampled_ms > 1)
{
volk_gnsssdr_free(d_fft_code_I_B);
volk_gnsssdr_free(d_magnitudeIB);
if (d_both_signal_components == true)
{
volk_gnsssdr_free(d_fft_code_Q_B);
volk_gnsssdr_free(d_magnitudeQB);
}
}
if (d_CAF_window_hz > 0)
{
volk_gnsssdr_free(d_CAF_vector);
volk_gnsssdr_free(d_CAF_vector_I);
if (d_both_signal_components == true)
{
volk_gnsssdr_free(d_CAF_vector_Q);
}
}
try
{
if (d_dump)
@ -236,8 +178,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_I_A, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_I_A.data(), d_fft_if->get_outbuf(), d_fft_size);
// SAME FOR PILOT SIGNAL
if (d_both_signal_components == true)
@ -247,8 +189,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_Q_A, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_Q_A.data(), d_fft_if->get_outbuf(), d_fft_size);
}
// IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination
// Note: max integration time allowed = 3ms (dealt in adapter)
@ -261,8 +203,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_I_B, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_I_B.data(), d_fft_if->get_outbuf(), d_fft_size);
if (d_both_signal_components == true)
{
@ -272,8 +214,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_Q_B, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_Q_B.data(), d_fft_if->get_outbuf(), d_fft_size);
}
}
}
@ -304,26 +246,24 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GALILEO_TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}
/* CAF Filtering to resolve doppler ambiguity. Phase and quadrature must be processed
* separately before non-coherent integration */
// if (d_CAF_filter)
if (d_CAF_window_hz > 0)
{
d_CAF_vector = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
d_CAF_vector_I = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
d_CAF_vector.reserve(d_num_doppler_bins);
d_CAF_vector_I.reserve(d_num_doppler_bins);
if (d_both_signal_components == true)
{
d_CAF_vector_Q = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
d_CAF_vector_Q.reserve(d_num_doppler_bins);
}
}
}
@ -369,8 +309,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
* 7. Declare positive or negative acquisition using a message port
*/
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
/* States: 0 Stop Channel
int acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
/* States: 0 Stop Channel
* 1 Load the buffer until it reaches fft_size
* 2 Acquisition algorithm
* 3 Positive acquisition
@ -382,7 +322,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
{
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -400,7 +340,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
case 1:
{
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
unsigned int buff_increment;
if ((ninput_items[0] + d_buffer_count) <= d_fft_size)
{
@ -424,7 +364,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
case 2:
{
// Fill last part of the buffer and reset counter
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
if (d_buffer_count < d_fft_size)
{
memcpy(&d_inbuffer[d_buffer_count], in, sizeof(gr_complex) * (d_fft_size - d_buffer_count));
@ -455,19 +395,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_inbuffer, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitudeIA.data(), d_inbuffer.data(), d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer.data(),
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
@ -477,14 +416,14 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_I_A, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_I_A.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_IA, d_magnitudeIA, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitudeIA.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_IA, d_magnitudeIA.data(), d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor);
@ -492,30 +431,30 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
{
// REPEAT FOR ALL CODES. CODE_QA
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_Q_A, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_Q_A.data(), d_fft_size);
d_ifft->execute();
volk_32fc_magnitude_squared_32f(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_QA, d_magnitudeQA, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitudeQA.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_QA, d_magnitudeQA.data(), d_fft_size);
magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor);
}
if (d_sampled_ms > 1) // If Integration time > 1 code
{
// REPEAT FOR ALL CODES. CODE_IB
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_I_B, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_I_B.data(), d_fft_size);
d_ifft->execute();
volk_32fc_magnitude_squared_32f(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_IB, d_magnitudeIB, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitudeIB.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_IB, d_magnitudeIB.data(), d_fft_size);
magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor);
if (d_both_signal_components == true)
{
// REPEAT FOR ALL CODES. CODE_QB
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_Q_B, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_Q_B.data(), d_fft_size);
d_ifft->execute();
volk_32fc_magnitude_squared_32f(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_QB, d_magnitudeQB, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitudeQB.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_QB, d_magnitudeQB.data(), d_fft_size);
magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor);
}
}
@ -528,7 +467,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
{
if (magt_IA >= magt_IB)
{
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IA;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
@ -538,7 +476,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// Integrate non-coherently I+Q
if (magt_QA >= magt_QB)
{
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
@ -550,7 +487,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
else
{
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QB;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
@ -561,12 +497,11 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
}
}
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA.data(), d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
}
else
{
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IB;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_I[doppler_index] = d_magnitudeIB[indext_IB];
@ -576,7 +511,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// Integrate non-coherently I+Q
if (magt_QA >= magt_QB)
{
//if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
@ -588,7 +522,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
else
{
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QB;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
@ -599,20 +532,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
}
}
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIB, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIB.data(), d_fft_size);
magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor);
}
}
else
{
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IA;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
}
if (d_both_signal_components)
{
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
if (d_CAF_window_hz > 0)
{
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
@ -623,7 +554,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_magnitudeIA[i] += d_magnitudeQA[i];
}
}
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA.data(), d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
}
@ -662,16 +593,16 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
{
if (magt_IA >= magt_IB)
{
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA), n);
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA.data()), n);
}
else
{
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIB), n);
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIB.data()), n);
}
}
else
{
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA), n);
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA.data()), n);
}
d_dump_file.close();
}
@ -681,7 +612,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
if (d_CAF_window_hz > 0)
{
int CAF_bins_half;
auto *accum = static_cast<float *>(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment()));
std::array<float, 1> accum{};
CAF_bins_half = d_CAF_window_hz / (2 * d_doppler_step);
float weighting_factor;
weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
@ -691,22 +622,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
for (int doppler_index = 0; doppler_index < CAF_bins_half; doppler_index++)
{
d_CAF_vector[doppler_index] = 0;
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], d_CAF_vector_I, CAF_bins_half+doppler_index+1);
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
{
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
}
// d_CAF_vector[doppler_index] /= CAF_bins_half+doppler_index+1;
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
if (d_both_signal_components)
{
accum[0] = 0;
// volk_32f_accumulator_s32f_a(&accum[0], d_CAF_vector_Q, CAF_bins_half+doppler_index+1);
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
{
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
}
// accum[0] /= CAF_bins_half+doppler_index+1;
accum[0] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
d_CAF_vector[doppler_index] += accum[0];
}
@ -715,22 +642,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
for (unsigned int doppler_index = CAF_bins_half; doppler_index < d_num_doppler_bins - CAF_bins_half; doppler_index++)
{
d_CAF_vector[doppler_index] = 0;
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], 2*CAF_bins_half+1);
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
{
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
}
// d_CAF_vector[doppler_index] /= 2*CAF_bins_half+1;
d_CAF_vector[doppler_index] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
if (d_both_signal_components)
{
accum[0] = 0;
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], 2*CAF_bins_half);
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
{
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
}
// accum[0] /= 2*CAF_bins_half+1;
accum[0] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
d_CAF_vector[doppler_index] += accum[0];
}
@ -739,29 +662,25 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
for (int doppler_index = d_num_doppler_bins - CAF_bins_half; doppler_index < static_cast<int>(d_num_doppler_bins); doppler_index++)
{
d_CAF_vector[doppler_index] = 0;
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
{
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * (abs(doppler_index - i)));
}
// d_CAF_vector[doppler_index] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
if (d_both_signal_components)
{
accum[0] = 0;
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
{
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * (abs(doppler_index - i)));
}
// accum[0] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
accum[0] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
d_CAF_vector[doppler_index] += accum[0];
}
}
// Recompute the maximum doppler peak
volk_gnsssdr_32f_index_max_32u(&indext, d_CAF_vector, d_num_doppler_bins);
volk_gnsssdr_32f_index_max_32u(&indext, d_CAF_vector.data(), d_num_doppler_bins);
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * indext;
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
// Dump if required, appended at the end of the file
@ -772,10 +691,9 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
filename.str("");
filename << "../data/test_statistics_E5a_sat_" << d_gnss_synchro->PRN << "_CAF.dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_CAF_vector), n);
d_dump_file.write(reinterpret_cast<char *>(d_CAF_vector.data()), n);
d_dump_file.close();
}
volk_gnsssdr_free(accum);
}
if (d_well_count == d_max_dwells)

27
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h
View File

@ -47,6 +47,7 @@
#include <memory>
#include <string>
#include <utility>
#include <vector>
class galileo_e5a_noncoherentIQ_acquisition_caf_cc;
@ -221,23 +222,23 @@ private:
unsigned int d_well_count;
unsigned int d_fft_size;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_code_I_A;
gr_complex* d_fft_code_I_B;
gr_complex* d_fft_code_Q_A;
gr_complex* d_fft_code_Q_B;
gr_complex* d_inbuffer;
std::vector<gr_complex> d_fft_code_I_A;
std::vector<gr_complex> d_fft_code_I_B;
std::vector<gr_complex> d_fft_code_Q_A;
std::vector<gr_complex> d_fft_code_Q_B;
std::vector<gr_complex> d_inbuffer;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitudeIA;
float* d_magnitudeIB;
float* d_magnitudeQA;
float* d_magnitudeQB;
std::vector<float> d_magnitudeIA;
std::vector<float> d_magnitudeIB;
std::vector<float> d_magnitudeQA;
std::vector<float> d_magnitudeQB;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;
@ -247,9 +248,9 @@ private:
bool d_dump;
bool d_both_signal_components;
int d_CAF_window_hz;
float* d_CAF_vector;
float* d_CAF_vector_I;
float* d_CAF_vector_Q;
std::vector<float> d_CAF_vector;
std::vector<float> d_CAF_vector_I;
std::vector<float> d_CAF_vector_Q;
unsigned int d_channel;
std::string d_dump_filename;
unsigned int d_buffer_count;

58
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@ -82,9 +82,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_input_power = 0.0;
d_num_doppler_bins = 0;
d_fft_code_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_code_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_code_A = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
d_fft_code_B = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
d_magnitude = std::vector<float>(d_fft_size, 0.0F);
// Direct FFT
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
@ -99,7 +99,6 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 0;
d_grid_doppler_wipeoffs = nullptr;
d_gnss_synchro = nullptr;
d_code_phase = 0;
d_doppler_freq = 0;
@ -110,19 +109,6 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
{
if (d_num_doppler_bins > 0)
{
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
volk_gnsssdr_free(d_fft_code_A);
volk_gnsssdr_free(d_fft_code_B);
volk_gnsssdr_free(d_magnitude);
try
{
if (d_dump)
@ -148,8 +134,8 @@ void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> *code)
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_A.data(), d_fft_if->get_outbuf(), d_fft_size);
// code B: two replicas of a primary code; the second replica is inverted.
volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
@ -158,8 +144,8 @@ void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> *code)
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_B.data(), d_fft_if->get_outbuf(), d_fft_size);
}
@ -184,16 +170,14 @@ void galileo_pcps_8ms_acquisition_cc::init()
{
d_num_doppler_bins++;
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}
}
@ -226,7 +210,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
@ -234,7 +218,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
{
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -263,7 +247,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
float magt = 0.0;
float magt_A = 0.0;
float magt_B = 0.0;
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
@ -279,8 +263,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
@ -290,7 +274,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
@ -300,14 +284,14 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// with the local FFT'd code A reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_A, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_A.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude.data(), d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor);
@ -316,14 +300,14 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// with the local FFT'd code B reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_B, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_B.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude.data(), d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor);

11
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h
View File

@ -41,6 +41,7 @@
#include <memory>
#include <string>
#include <utility>
#include <vector>
class galileo_pcps_8ms_acquisition_cc;
@ -206,17 +207,17 @@ private:
uint32_t d_well_count;
uint32_t d_fft_size;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
uint32_t d_num_doppler_bins;
gr_complex* d_fft_code_A;
gr_complex* d_fft_code_B;
std::vector<gr_complex> d_fft_code_A;
std::vector<gr_complex> d_fft_code_B;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
uint32_t d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
std::vector<float> d_magnitude;
float d_input_power;
float d_test_statistics;
std::ofstream d_dump_file;
@ -228,4 +229,4 @@ private:
std::string d_dump_filename;
};
#endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_*/
#endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_ */

35
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@ -43,6 +43,7 @@
#include <sstream>
#include <utility>
extern Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
@ -79,8 +80,7 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_input_power = 0.0;
d_state = 0;
d_disable_assist = false;
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_codes.reserve(d_fft_size);
// Direct FFT
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
@ -115,8 +115,6 @@ void pcps_assisted_acquisition_cc::set_doppler_step(uint32_t doppler_step)
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{
volk_gnsssdr_free(d_carrier);
volk_gnsssdr_free(d_fft_codes);
try
{
if (d_dump)
@ -156,8 +154,8 @@ void pcps_assisted_acquisition_cc::init()
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
}
@ -279,10 +277,10 @@ double pcps_assisted_acquisition_cc::search_maximum()
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler].data()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler].data()), n); // write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
@ -292,16 +290,14 @@ double pcps_assisted_acquisition_cc::search_maximum()
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
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
// 1- Compute the input signal power estimation
auto *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
std::vector<float> p_tmp_vector(d_fft_size);
volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), in, d_fft_size);
const float *p_const_tmp_vector = p_tmp_vector;
float power;
volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
volk_gnsssdr_free(p_tmp_vector);
volk_32f_accumulator_s32f(&power, p_tmp_vector.data(), d_fft_size);
return (power / static_cast<float>(d_fft_size));
}
@ -309,7 +305,7 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{
// initialize acquisition algorithm
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "
@ -319,7 +315,7 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons
<< ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop
auto *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
std::vector<float> p_tmp_vector(d_fft_size);
for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{
@ -332,17 +328,16 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// save the grid matrix delay file
volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
volk_32fc_magnitude_squared_32f(p_tmp_vector.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, d_fft_size);
volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector.data(), d_fft_size);
}
volk_gnsssdr_free(p_tmp_vector);
return d_fft_size;
}

5
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h
View File

@ -215,8 +215,7 @@ private:
uint32_t d_sampled_ms;
uint32_t d_fft_size;
uint64_t d_sample_counter;
gr_complex* d_carrier;
gr_complex* d_fft_codes;
std::vector<gr_complex> d_fft_codes;
std::vector<std::vector<float>> d_grid_data;
std::vector<std::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
@ -239,4 +238,4 @@ private:
std::string d_dump_filename;
};
#endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/
#endif /* GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_ */

79
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@ -88,13 +88,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_input_power = 0.0;
d_num_doppler_bins = 0;
d_fft_code_data = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_code_pilot = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_data_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_pilot_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_correlation_plus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_correlation_minus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_code_data.reserve(d_fft_size);
d_fft_code_pilot.reserve(d_fft_size);
d_data_correlation.reserve(d_fft_size);
d_pilot_correlation.reserve(d_fft_size);
d_correlation_plus.reserve(d_fft_size);
d_correlation_minus.reserve(d_fft_size);
d_magnitude.reserve(d_fft_size);
// Direct FFT
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
@ -109,7 +109,6 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 0;
d_grid_doppler_wipeoffs = nullptr;
d_gnss_synchro = nullptr;
d_code_phase = 0;
d_doppler_freq = 0;
@ -120,23 +119,6 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
{
if (d_num_doppler_bins > 0)
{
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
volk_gnsssdr_free(d_fft_code_data);
volk_gnsssdr_free(d_fft_code_pilot);
volk_gnsssdr_free(d_data_correlation);
volk_gnsssdr_free(d_pilot_correlation);
volk_gnsssdr_free(d_correlation_plus);
volk_gnsssdr_free(d_correlation_minus);
volk_gnsssdr_free(d_magnitude);
try
{
if (d_dump)
@ -163,16 +145,16 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> *code_data,
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_data, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_data.data(), d_fft_if->get_outbuf(), d_fft_size);
// Pilot code (E1C)
memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code,
volk_32fc_conjugate_32fc(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code,
volk_32fc_conjugate_32fc(d_fft_code_pilot.data(), d_fft_if->get_outbuf(), d_fft_size);
}
@ -199,15 +181,13 @@ void pcps_cccwsr_acquisition_cc::init()
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}
}
@ -240,7 +220,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
@ -248,7 +228,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
{
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -277,7 +257,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
float magt = 0.0;
float magt_plus = 0.0;
float magt_minus = 0.0;
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_sample_counter += static_cast<uint64_t>(d_fft_size); // sample counter
@ -291,19 +271,18 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
@ -313,27 +292,27 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
// with the local FFT'd data code reference (E1B) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_data, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_data.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and data code in
// d_data_correlation.
memcpy(d_data_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
memcpy(d_data_correlation.data(), d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd pilot code reference (E1C) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_pilot, d_fft_size);
d_fft_if->get_outbuf(), d_fft_code_pilot.data(), d_fft_size);
// Compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and pilot code in
// d_data_correlation.
memcpy(d_pilot_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
memcpy(d_pilot_correlation.data(), d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
for (uint32_t i = 0; i < d_fft_size; i++)
{
@ -346,12 +325,12 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
d_data_correlation[i].imag() - d_pilot_correlation[i].real());
}
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_correlation_plus.data(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude.data(), d_fft_size);
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_correlation_minus.data(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude.data(), d_fft_size);
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
if (magt_plus >= magt_minus)
@ -382,10 +361,10 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); // write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}

19
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h
View File

@ -46,6 +46,7 @@
#include <memory>
#include <string>
#include <utility>
#include <vector>
class pcps_cccwsr_acquisition_cc;
@ -201,21 +202,21 @@ private:
uint32_t d_well_count;
uint32_t d_fft_size;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
uint32_t d_num_doppler_bins;
gr_complex* d_fft_code_data;
gr_complex* d_fft_code_pilot;
std::vector<gr_complex> d_fft_code_data;
std::vector<gr_complex> d_fft_code_pilot;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
uint32_t d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
gr_complex* d_data_correlation;
gr_complex* d_pilot_correlation;
gr_complex* d_correlation_plus;
gr_complex* d_correlation_minus;
std::vector<float> d_magnitude;
std::vector<gr_complex> d_data_correlation;
std::vector<gr_complex> d_pilot_correlation;
std::vector<gr_complex> d_correlation_plus;
std::vector<gr_complex> d_correlation_minus;
float d_input_power;
float d_test_statistics;
std::ofstream d_dump_file;
@ -227,4 +228,4 @@ private:
std::string d_dump_filename;
};
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_ */

110
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@ -112,19 +112,10 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_in_dwell_count = 0;
d_cl_fft_batch_size = 1;
d_in_buffer = new gr_complex *[d_max_dwells];
for (uint32_t i = 0; i < d_max_dwells; i++)
{
d_in_buffer[i] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
}
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_zero_vector = static_cast<gr_complex *>(volk_gnsssdr_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
for (uint32_t i = 0; i < (d_fft_size_pow2 - d_fft_size); i++)
{
d_zero_vector[i] = gr_complex(0.0, 0.0);
}
d_in_buffer = std::vector<std::vector<gr_complex>>(d_max_dwells, std::vector<gr_complex>(d_fft_size));
d_magnitude.reserve(d_fft_size);
d_fft_codes.reserve(d_fft_size_pow2);
d_zero_vector = std::vector<gr_complex>(d_fft_size_pow2 - d_fft_size, 0.0);
d_opencl = init_opencl_environment("math_kernel.cl");
@ -145,25 +136,6 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
{
if (d_num_doppler_bins > 0)
{
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
for (uint32_t i = 0; i < d_max_dwells; i++)
{
volk_gnsssdr_free(d_in_buffer[i]);
}
delete[] d_in_buffer;
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
volk_gnsssdr_free(d_zero_vector);
if (d_opencl == 0)
{
delete d_cl_queue;
@ -200,7 +172,7 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kernel_filename)
{
//get all platforms (drivers)
// get all platforms (drivers)
std::vector<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms);
@ -210,11 +182,11 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
return 1;
}
d_cl_platform = all_platforms[0]; //get default platform
d_cl_platform = all_platforms[0]; // get default platform
std::cout << "Using platform: " << d_cl_platform.getInfo<CL_PLATFORM_NAME>()
<< std::endl;
//get default GPU device of the default platform
// get default GPU device of the default platform
std::vector<cl::Device> gpu_devices;
d_cl_platform.getDevices(CL_DEVICE_TYPE_GPU, &gpu_devices);
@ -239,8 +211,6 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
(std::istreambuf_iterator<char>()));
kernel_file.close();
// std::cout << "Kernel code: \n" << kernel_code << std::endl;
cl::Program::Sources sources;
sources.push_back({kernel_code.c_str(), kernel_code.length()});
@ -262,10 +232,10 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float) * d_fft_size);
//create queue to which we will push commands for the device.
// create queue to which we will push commands for the device.
d_cl_queue = new cl::CommandQueue(d_cl_context, d_cl_device);
//create FFT plan
// create FFT plan
cl_int err;
clFFT_Dim3 dim = {d_fft_size_pow2, 1, 1};
@ -312,7 +282,7 @@ void pcps_opencl_acquisition_cc::init()
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
if (d_opencl == 0)
{
d_cl_buffer_grid_doppler_wipeoffs = new cl::Buffer *[d_num_doppler_bins];
@ -320,12 +290,10 @@ void pcps_opencl_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
if (d_opencl == 0)
{
@ -334,7 +302,7 @@ void pcps_opencl_acquisition_cc::init()
d_cl_queue->enqueueWriteBuffer(*(d_cl_buffer_grid_doppler_wipeoffs[doppler_index]),
CL_TRUE, 0, sizeof(gr_complex) * d_fft_size,
d_grid_doppler_wipeoffs[doppler_index]);
d_grid_doppler_wipeoffs[doppler_index].data());
}
}
@ -342,7 +310,7 @@ void pcps_opencl_acquisition_cc::init()
if (d_opencl == 0)
{
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_1, CL_TRUE, sizeof(gr_complex) * d_fft_size,
sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size), d_zero_vector);
sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size), d_zero_vector.data());
}
}
@ -356,7 +324,7 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * d_fft_size,
sizeof(gr_complex) * (d_fft_size_pow2 - 2 * d_fft_size),
d_zero_vector);
d_zero_vector.data());
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size),
sizeof(gr_complex) * d_fft_size, code);
@ -365,10 +333,10 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
0, nullptr, nullptr);
//Conjucate the local code
// Conjucate the local code
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
kernel.setArg(0, *d_cl_buffer_2); //input
kernel.setArg(1, *d_cl_buffer_fft_codes); //output
kernel.setArg(0, *d_cl_buffer_2); // input
kernel.setArg(1, *d_cl_buffer_fft_codes); // output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
}
else
@ -377,8 +345,8 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
}
}
@ -390,7 +358,6 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
uint32_t indext = 0;
float magt = 0.0;
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
gr_complex *in = d_in_buffer[d_well_count];
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
d_input_power = 0.0;
@ -405,8 +372,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_in_buffer[d_well_count].data(), d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
@ -415,8 +382,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_in_buffer[d_well_count].data(),
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
@ -425,14 +392,14 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
d_fft_if->get_outbuf(), d_fft_codes.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude.data(), d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@ -469,7 +436,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
@ -514,14 +481,13 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
uint32_t indext = 0;
float magt = 0.0;
float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why.
gr_complex *in = d_in_buffer[d_well_count];
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
d_input_power = 0.0;
d_mag = 0.0;
// write input vector in buffer of OpenCL device
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size, in);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size, d_in_buffer[d_well_count].data());
d_well_count++;
@ -539,8 +505,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_in_buffer[d_well_count].data(), d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
cl::Kernel kernel;
@ -549,10 +515,9 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
//Multiply input signal with doppler wipe-off
// Multiply input signal with doppler wipe-off
kernel = cl::Kernel(d_cl_program, "mult_vectors");
kernel.setArg(0, *d_cl_buffer_in); //input 1
kernel.setArg(1, *d_cl_buffer_grid_doppler_wipeoffs[doppler_index]); //input 2
@ -563,7 +528,6 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// In the previous operation, we store the result in the first d_fft_size positions
// of d_cl_buffer_1. The rest d_fft_size_pow2-d_fft_size already have zeros
// (zero-padding is made in init() for optimization purposes).
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Forward, (*d_cl_buffer_1)(), (*d_cl_buffer_2)(),
0, nullptr, nullptr);
@ -592,11 +556,11 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// This is the only function that blocks this thread until all previously enqueued
// OpenCL commands are completed.
d_cl_queue->enqueueReadBuffer(*d_cl_buffer_magnitude, CL_TRUE, 0,
sizeof(float) * d_fft_size, d_magnitude);
sizeof(float) * d_fft_size, d_magnitude.data());
// Search maximum
// @TODO: find an efficient way to search the maximum with OpenCL in the GPU.
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude.data(), d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@ -633,7 +597,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
@ -705,14 +669,14 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
case 0:
{
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -742,7 +706,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
uint32_t num_dwells = std::min(static_cast<int>(d_max_dwells - d_in_dwell_count), ninput_items[0]);
for (uint32_t i = 0; i < num_dwells; i++)
{
memcpy(d_in_buffer[d_in_dwell_count++], static_cast<const gr_complex *>(input_items[i]),
memcpy(d_in_buffer[d_in_dwell_count++].data(), static_cast<const gr_complex *>(input_items[i]),
sizeof(gr_complex) * d_fft_size);
d_sample_counter += static_cast<uint64_t>(d_fft_size);
d_sample_counter_buffer.push_back(d_sample_counter);

10
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h
View File

@ -241,16 +241,16 @@ private:
uint32_t d_fft_size_pow2;
int* d_max_doppler_indexs;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
uint32_t d_num_doppler_bins;
gr_complex* d_fft_codes;
std::vector<gr_complex> d_fft_codes;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
uint32_t d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
std::vector<float> d_magnitude;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;
@ -261,8 +261,8 @@ private:
bool d_dump;
uint32_t d_channel;
std::string d_dump_filename;
gr_complex* d_zero_vector;
gr_complex** d_in_buffer;
std::vector<gr_complex> d_zero_vector;
std::vector<std::vector<gr_complex>> d_in_buffer;
std::vector<uint64_t> d_sample_counter_buffer;
uint32_t d_in_dwell_count;
std::weak_ptr<ChannelFsm> d_channel_fsm;

136
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@ -91,19 +91,19 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
d_bit_transition_flag = bit_transition_flag;
d_folding_factor = folding_factor;
//fft size is reduced.
// fft size is reduced.
d_fft_size = (d_samples_per_code) / d_folding_factor;
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_gnsssdr_get_alignment()));
d_magnitude_folded = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_codes.reserve(d_fft_size);
d_magnitude.reserve(d_samples_per_code * d_folding_factor);
d_magnitude_folded.reserve(d_fft_size);
d_possible_delay = new uint32_t[d_folding_factor];
d_corr_output_f = new float[d_folding_factor];
d_possible_delay.reserve(d_folding_factor);
d_corr_output_f.reserve(d_folding_factor);
/*Create the d_code signal , which would store the values of the code in its
original form to perform later correlation in time domain*/
d_code = new gr_complex[d_samples_per_code]();
d_code = std::vector<gr_complex>(d_samples_per_code, lv_cmake(0.0F, 0.0F));
// Direct FFT
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
@ -114,46 +114,22 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
d_dump = dump;
d_dump_filename = std::move(dump_filename);
d_corr_acumulator = nullptr;
d_signal_folded = nullptr;
d_code_folded = new gr_complex[d_fft_size]();
d_code_folded = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
d_signal_folded.reserve(d_fft_size);
d_noise_floor_power = 0;
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 0;
d_grid_doppler_wipeoffs = nullptr;
d_gnss_synchro = nullptr;
d_code_phase = 0;
d_doppler_freq = 0;
d_test_statistics = 0;
d_channel = 0;
//d_code_folded = 0;
// DLOG(INFO) << "END CONSTRUCTOR";
}
pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
{
//DLOG(INFO) << "START DESTROYER";
if (d_num_doppler_bins > 0)
{
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
volk_gnsssdr_free(d_magnitude_folded);
delete d_code;
delete d_possible_delay;
delete d_corr_output_f;
delete[] d_code_folded;
try
{
if (d_dump)
@ -174,16 +150,15 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
{
/*save a local copy of the code without the folding process to perform corre-
lation in time in the final steps of the acquisition stage*/
memcpy(d_code, code, sizeof(gr_complex) * d_samples_per_code);
/* save a local copy of the code without the folding process to perform corre-
lation in time in the final steps of the acquisition stage */
memcpy(d_code.data(), code, sizeof(gr_complex) * d_samples_per_code);
//d_code_folded = new gr_complex[d_fft_size]();
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex) * (d_fft_size));
memcpy(d_fft_if->get_inbuf(), d_code_folded.data(), sizeof(gr_complex) * (d_fft_size));
/*perform folding of the code by the factorial factor parameter. Notice that
/* perform folding of the code by the factorial factor parameter. Notice that
folding of the code in the time stage would result in a downsampled spectrum
in the frequency domain after applying the fftw operation*/
in the frequency domain after applying the fftw operation */
for (uint32_t i = 0; i < d_folding_factor; i++)
{
std::transform((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)),
@ -193,8 +168,8 @@ void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
}
@ -204,8 +179,6 @@ void pcps_quicksync_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
//DLOG(INFO) << "START init";
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -228,16 +201,14 @@ void pcps_quicksync_acquisition_cc::init()
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_samples_per_code * d_folding_factor));
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_samples_per_code * d_folding_factor);
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_samples_per_code * d_folding_factor);
}
// DLOG(INFO) << "end init";
}
@ -279,17 +250,16 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
* 5. Compute the test statistics and compare to the threshold
* 6. Declare positive or negative acquisition using a message queue
*/
//DLOG(INFO) << "START GENERAL WORK";
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
//std::cout<<"general_work in quicksync gnuradio block"<<std::endl;
// DLOG(INFO) << "START GENERAL WORK";
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
case 0:
{
//DLOG(INFO) << "START CASE 0";
// DLOG(INFO) << "START CASE 0";
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -304,29 +274,27 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
d_sample_counter += static_cast<uint64_t>(d_sampled_ms * d_samples_per_ms * ninput_items[0]); // sample counter
consume_each(ninput_items[0]);
//DLOG(INFO) << "END CASE 0";
// DLOG(INFO) << "END CASE 0";
break;
}
case 1:
{
// initialize acquisition implementing the QuickSync algorithm
//DLOG(INFO) << "START CASE 1";
// DLOG(INFO) << "START CASE 1";
int32_t doppler;
uint32_t indext = 0;
float magt = 0.0;
const auto* in = reinterpret_cast<const gr_complex*>(input_items[0]); // Get the input samples pointer
auto* in_temp = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
auto* in_temp_folded = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
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
auto* in_1code = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
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
auto* corr_output = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
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*/
@ -355,8 +323,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
// 1- Compute the input signal power estimation. This operation is
// being performed in a signal of size nxp
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_samples_per_code * d_folding_factor);
d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor);
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
@ -364,8 +332,8 @@ 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 acumulation when performing
// the folding process to be stored in d_fft_if->get_inbuf()
d_signal_folded = new gr_complex[d_fft_size]();
memcpy(d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
d_signal_folded = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
memcpy(d_fft_if->get_inbuf(), d_signal_folded.data(), sizeof(gr_complex) * (d_fft_size));
// Doppler search steps and then multiplication of the incoming
// signal with the doppler wipeoffs to eliminate frequency offset
@ -374,8 +342,8 @@ 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, in,
d_grid_doppler_wipeoffs[doppler_index],
volk_32fc_x2_multiply_32fc(in_temp.data(), in,
d_grid_doppler_wipeoffs[doppler_index].data(),
d_samples_per_code * d_folding_factor);
// Perform folding of the carrier wiped-off incoming signal. Since
@ -383,8 +351,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 + i * d_fft_size),
(in_temp + ((i + 1) * d_fft_size)),
std::transform((in_temp.data() + i * d_fft_size),
(in_temp.data() + ((i + 1) * d_fft_size)),
d_fft_if->get_inbuf(),
d_fft_if->get_inbuf(),
std::plus<gr_complex>());
@ -398,24 +366,22 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
// signal with the local FFT'd code reference using SIMD
// operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
d_fft_if->get_outbuf(), d_fft_codes.data(), d_fft_size);
// compute the inverse FFT of the aliased signal
d_ifft->execute();
// Compute the magnitude and get the maximum value with its
// index position
volk_32fc_magnitude_squared_32f(d_magnitude_folded,
volk_32fc_magnitude_squared_32f(d_magnitude_folded.data(),
d_ifft->get_outbuf(), d_fft_size);
// Normalize the maximum value to correct the scale factor
// introduced by FFTW
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded.data(), d_fft_size);
magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor);
delete[] d_signal_folded;
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
@ -443,7 +409,7 @@ 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*/
memcpy(in_1code, &in_temp[d_possible_delay[i]],
memcpy(in_1code.data(), &in_temp[d_possible_delay[i]],
sizeof(gr_complex) * (d_samples_per_code));
// Perform multiplication of the unmodified local
@ -451,7 +417,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
// 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, in_1code, d_code, d_samples_per_code);
volk_32fc_x2_multiply_32fc(corr_output.data(), in_1code.data(), d_code.data(), d_samples_per_code);
for (int32_t j = 0; j < d_samples_per_code; j++)
{
@ -459,8 +425,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
}
}
// Obtain maximum value of correlation given the possible delay selected
volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator.data(), d_folding_factor);
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f, d_folding_factor);
volk_32fc_magnitude_squared_32f(d_corr_output_f.data(), complex_acumulator.data(), d_folding_factor);
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f.data(), d_folding_factor);
// Now save the real code phase in the gnss_syncro block for use in other stages
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
@ -483,10 +449,10 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded), n); // write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded.data()), n); // write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
@ -517,10 +483,6 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
}
}
volk_gnsssdr_free(in_temp);
volk_gnsssdr_free(in_temp_folded);
volk_gnsssdr_free(in_1code);
volk_gnsssdr_free(corr_output);
consume_each(1);
break;
@ -528,7 +490,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
case 2:
{
//DLOG(INFO) << "START CASE 2";
// DLOG(INFO) << "START CASE 2";
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
@ -554,13 +516,13 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 2";
// DLOG(INFO) << "END CASE 2";
break;
}
case 3:
{
//DLOG(INFO) << "START CASE 3";
// DLOG(INFO) << "START CASE 3";
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
@ -586,7 +548,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 3";
// DLOG(INFO) << "END CASE 3";
break;
}
}

20
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h
View File

@ -62,6 +62,7 @@
#include <functional>
#include <string>
#include <utility>
#include <vector>
class pcps_quicksync_acquisition_cc;
@ -213,14 +214,13 @@ private:
void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift,
int32_t doppler_offset);
gr_complex* d_code;
std::vector<gr_complex> d_code;
uint32_t d_folding_factor; // also referred in the paper as 'p'
float* d_corr_acumulator;
uint32_t* d_possible_delay;
float* d_corr_output_f;
float* d_magnitude_folded;
gr_complex* d_signal_folded;
gr_complex* d_code_folded;
std::vector<uint32_t> d_possible_delay;
std::vector<float> d_corr_output_f;
std::vector<float> d_magnitude_folded;
std::vector<gr_complex> d_signal_folded;
std::vector<gr_complex> d_code_folded;
float d_noise_floor_power;
int64_t d_fs_in;
int32_t d_samples_per_ms;
@ -235,16 +235,16 @@ private:
uint32_t d_well_count;
uint32_t d_fft_size;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
uint32_t d_num_doppler_bins;
gr_complex* d_fft_codes;
std::vector<gr_complex> d_fft_codes;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
uint32_t d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
std::vector<float> d_magnitude;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;

70
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@ -109,8 +109,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_input_power = 0.0;
d_num_doppler_bins = 0;
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_codes.reserve(d_fft_size);
d_magnitude.reserve(d_fft_size);
// Direct FFT
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
@ -125,8 +125,6 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 0;
d_grid_data = nullptr;
d_grid_doppler_wipeoffs = nullptr;
d_gnss_synchro = nullptr;
d_code_phase = 0;
d_doppler_freq = 0;
@ -137,20 +135,6 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
{
if (d_num_doppler_bins > 0)
{
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
volk_gnsssdr_free(d_grid_data[i]);
}
delete[] d_grid_doppler_wipeoffs;
delete[] d_grid_data;
}
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
try
{
if (d_dump)
@ -175,8 +159,8 @@ void pcps_tong_acquisition_cc::set_local_code(std::complex<float> *code)
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
// Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
}
@ -203,23 +187,14 @@ void pcps_tong_acquisition_cc::init()
}
// Create the carrier Doppler wipeoff signals and allocate data grid.
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_data = new float *[d_num_doppler_bins];
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
d_grid_data = std::vector<std::vector<float>>(d_num_doppler_bins, std::vector<float>(d_fft_size, 0.0));
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
d_grid_data[doppler_index] = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
for (uint32_t i = 0; i < d_fft_size; i++)
{
d_grid_data[doppler_index][i] = 0;
}
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}
}
@ -243,7 +218,7 @@ void pcps_tong_acquisition_cc::set_state(int32_t state)
{
for (uint32_t i = 0; i < d_fft_size; i++)
{
d_grid_data[doppler_index][i] = 0;
d_grid_data[doppler_index][i] = 0.0;
}
}
}
@ -261,7 +236,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
@ -269,7 +244,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
{
if (d_active)
{
//restart acquisition variables
// restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -284,7 +259,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
{
for (uint32_t i = 0; i < d_fft_size; i++)
{
d_grid_data[doppler_index][i] = 0;
d_grid_data[doppler_index][i] = 0.0;
}
}
@ -303,7 +278,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
int32_t doppler;
uint32_t indext = 0;
float magt = 0.0;
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
@ -319,19 +294,18 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
@ -340,24 +314,24 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
d_fft_if->get_outbuf(), d_fft_codes.data(), d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Compute magnitude
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
// Compute vector of test statistics corresponding to current doppler index.
volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
volk_32f_s32f_multiply_32f(d_magnitude.data(), d_magnitude.data(),
1 / (fft_normalization_factor * fft_normalization_factor * d_input_power),
d_fft_size);
// Accumulate test statistics in d_grid_data.
volk_32f_x2_add_32f(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size);
volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_magnitude.data(), d_grid_data[doppler_index].data(), d_fft_size);
// Search maximum
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index], d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index].data(), d_fft_size);
magt = d_grid_data[doppler_index][indext];
@ -378,10 +352,10 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); // write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}

9
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h
View File

@ -59,6 +59,7 @@
#include <fstream>
#include <string>
#include <utility>
#include <vector>
class pcps_tong_acquisition_cc;
@ -220,17 +221,17 @@ private:
uint32_t d_tong_max_dwells;
uint32_t d_fft_size;
uint64_t d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
uint32_t d_num_doppler_bins;
gr_complex* d_fft_codes;
float** d_grid_data;
std::vector<gr_complex> d_fft_codes;
std::vector<std::vector<float>> d_grid_data;
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
std::shared_ptr<gr::fft::fft_complex> d_ifft;
Gnss_Synchro* d_gnss_synchro;
uint32_t d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
std::vector<float> d_magnitude;
float d_input_power;
float d_test_statistics;
std::ofstream d_dump_file;