mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Better VOLK usage. Memory alignment, using dispatcher instead of

Browse Source 
aligned/unaligned versions. Code cleaning.
This commit is contained in:
Carles Fernandez 2014-09-10 03:15:01 +02:00
parent fd6a8e3cff
commit 9106f147ef
11 changed files with 753 additions and 922 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -107,35 +107,27 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
d_both_signal_components = both_signal_components_; d_both_signal_components = both_signal_components_;
d_CAF_window_hz = CAF_window_hz_; d_CAF_window_hz = CAF_window_hz_;
//todo: do something if posix_memalign fails d_inbuffer = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_inbuffer, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_I_A = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_code_I_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitudeIA = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitudeIA, 16, d_fft_size * sizeof(float)) == 0){};
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_fft_code_Q_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_Q_A = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_magnitudeQA, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeQA = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
} }
// IF COHERENT INTEGRATION TIME > 1 // IF COHERENT INTEGRATION TIME > 1
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
if (posix_memalign((void**)&d_fft_code_I_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_I_B = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_magnitudeIB, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeIB = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_fft_code_Q_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_Q_B = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_magnitudeQB, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeQB = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
} }
} }
// if (posix_memalign((void**)&d_fft_code_Q_A, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeQA, 16, d_fft_size * sizeof(float)) == 0){};
// if (posix_memalign((void**)&d_fft_code_I_B, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeIB, 16, d_fft_size * sizeof(float)) == 0){};
// if (posix_memalign((void**)&d_fft_code_Q_B, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeQB, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -153,34 +145,43 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisi
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_I_A); volk_free(d_inbuffer);
free(d_magnitudeIA); volk_free(d_fft_code_I_A);
volk_free(d_magnitudeIA);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
free(d_fft_code_Q_A); volk_free(d_fft_code_Q_A);
free(d_magnitudeQA); volk_free(d_magnitudeQA);
} }
// IF INTEGRATION TIME > 1 // IF INTEGRATION TIME > 1
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
free(d_fft_code_I_B); volk_free(d_fft_code_I_B);
free(d_magnitudeIB); volk_free(d_magnitudeIB);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
free(d_fft_code_Q_B); volk_free(d_fft_code_Q_B);
free(d_magnitudeQB); volk_free(d_magnitudeQB);
} }
} }
if (d_CAF_window_hz > 0)
{
volk_free(d_CAF_vector);
volk_free(d_CAF_vector_I);
if (d_both_signal_components == true)
{
volk_free(d_CAF_vector_Q);
}
}
delete d_fft_if; delete d_fft_if;
delete d_ifft; delete d_ifft;
if (d_dump) if (d_dump)
{ {
d_dump_file.close(); d_dump_file.close();
@ -197,70 +198,45 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
}
// SAME FOR PILOT SIGNAL // SAME FOR PILOT SIGNAL
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// Three replicas of pilot primary code. CODE A: (1,1,1) // Three replicas of pilot primary code. CODE A: (1,1,1)
memcpy(d_fft_if->get_inbuf(), codeQ, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), codeQ, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
{ }
volk_32fc_conjugate_32fc_u(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
}
}
// IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination // IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination
// Note: max integration time allowed = 3ms (dealt in adapter) // Note: max integration time allowed = 3ms (dealt in adapter)
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
// DATA CODE B: First replica is inverted (0,1,1) // DATA CODE B: First replica is inverted (0,1,1)
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[0], volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[0],
&codeI[0], gr_complex(-1,0), &codeI[0], gr_complex(-1,0),
d_samples_per_code); d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code d_fft_if->execute(); // We need the FFT of local code
if (is_unaligned())
{
volk_32fc_conjugate_32fc_u(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
}
if (d_both_signal_components == true)
{
// PILOT CODE B: First replica is inverted (0,1,1)
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[0],
&codeQ[0], gr_complex(-1,0),
d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size); if (d_both_signal_components == true)
} {
else // PILOT CODE B: First replica is inverted (0,1,1)
{ volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[0],
volk_32fc_conjugate_32fc_a(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size); &codeQ[0], gr_complex(-1,0),
} 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);
}
}
} }
void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init() void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
@ -284,9 +260,7 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index; int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
@ -297,11 +271,11 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
// if (d_CAF_filter) // if (d_CAF_filter)
if (d_CAF_window_hz > 0) if (d_CAF_window_hz > 0)
{ {
if (posix_memalign((void**)&d_CAF_vector, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector = (float*)volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_CAF_vector_I, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector_I = (float*)volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment());
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_CAF_vector_Q, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector_Q = (float*)volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment());
} }
} }
} }
@ -408,18 +382,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_inbuffer, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_inbuffer, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitudeIA, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), d_inbuffer, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -429,46 +403,46 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// CODE IA // CODE IA
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_IA, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext_IA, d_magnitudeIA, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor); magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// REPEAT FOR ALL CODES. CODE_QA // REPEAT FOR ALL CODES. CODE_QA
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_QA, d_magnitudeQA, d_fft_size); volk_32f_index_max_16u(&indext_QA, d_magnitudeQA, d_fft_size);
magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor); magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor);
} }
if (d_sampled_ms > 1) // If Integration time > 1 code if (d_sampled_ms > 1) // If Integration time > 1 code
{ {
// REPEAT FOR ALL CODES. CODE_IB // REPEAT FOR ALL CODES. CODE_IB
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_IB, d_magnitudeIB, d_fft_size); volk_32f_index_max_16u(&indext_IB, d_magnitudeIB, d_fft_size);
magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor); magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// REPEAT FOR ALL CODES. CODE_QB // REPEAT FOR ALL CODES. CODE_QB
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_QB, d_magnitudeQB, d_fft_size); volk_32f_index_max_16u(&indext_QB, d_magnitudeQB, d_fft_size);
magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor); magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor);
} }
} }
@ -505,7 +479,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
} }
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIA, d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
else else
@ -534,7 +508,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
} }
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIB, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIB, d_fft_size);
magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
} }
@ -552,7 +526,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_magnitudeIA[i] += d_magnitudeQA[i]; d_magnitudeIA[i] += d_magnitudeQA[i];
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIA, d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
@ -609,104 +583,103 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 6 OPTIONAL: CAF filter to avoid Doppler ambiguity in bit transition. // 6 OPTIONAL: CAF filter to avoid Doppler ambiguity in bit transition.
if (d_CAF_window_hz > 0) if (d_CAF_window_hz > 0)
{ {
int CAF_bins_half; int CAF_bins_half;
float* accum; float* accum = (float*)volk_malloc(sizeof(float), volk_get_alignment());
// double* accum; CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step);
if (posix_memalign((void**)&accum, 16, sizeof(float)) == 0){}; float weighting_factor;
CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step); weighting_factor = 0.5/(float)CAF_bins_half;
float weighting_factor; // weighting_factor = 0;
weighting_factor = 0.5/(float)CAF_bins_half; // std::cout << "weighting_factor " << weighting_factor << std::endl;
// weighting_factor = 0; // Initialize first iterations
// std::cout << "weighting_factor " << weighting_factor << std::endl; for (int doppler_index=0;doppler_index<CAF_bins_half;doppler_index++)
// Initialize first iterations {
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);
d_CAF_vector[doppler_index] = 0; for (int i = 0; i < CAF_bins_half+doppler_index+1; i++)
// 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*(unsigned int)(abs(doppler_index - i)));
{ }
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(unsigned int)(abs(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]
// d_CAF_vector[doppler_index] /= CAF_bins_half+doppler_index+1; if (d_both_signal_components)
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);
accum[0] = 0; for (int i = 0; i < CAF_bins_half+doppler_index+1; i++)
// 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*(unsigned int)(abs(doppler_index - i)));
{ }
accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(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]
// accum[0] /= CAF_bins_half+doppler_index+1; d_CAF_vector[doppler_index] += accum[0];
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]; }
} // Body loop
} for (unsigned int doppler_index=CAF_bins_half;doppler_index<d_num_doppler_bins-CAF_bins_half;doppler_index++)
// Body loop {
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);
d_CAF_vector[doppler_index] = 0; for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++)
// 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 < doppler_index+CAF_bins_half+1; i++) d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i)));
{ }
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(unsigned int)(abs(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;
// d_CAF_vector[doppler_index] /= 2*CAF_bins_half+1; if (d_both_signal_components)
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);
accum[0] = 0; for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++)
// 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 < doppler_index+CAF_bins_half+1; i++) accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i)));
{ }
accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(unsigned int)(abs(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;
// accum[0] /= 2*CAF_bins_half+1; d_CAF_vector[doppler_index] += accum[0];
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]; }
} // Final iterations
} for (unsigned int doppler_index=d_num_doppler_bins-CAF_bins_half;doppler_index<d_num_doppler_bins;doppler_index++)
// Final iterations {
for (unsigned int doppler_index=d_num_doppler_bins-CAF_bins_half;doppler_index<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));
d_CAF_vector[doppler_index] = 0; for (int i = doppler_index-CAF_bins_half; i < d_num_doppler_bins; i++)
// 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 < 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] += 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;
// d_CAF_vector[doppler_index] /= CAF_bins_half+(d_num_doppler_bins-doppler_index); if (d_both_signal_components)
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));
accum[0] = 0; for (int i = doppler_index-CAF_bins_half; i < d_num_doppler_bins; i++)
// 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 < d_num_doppler_bins; i++) accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(abs(doppler_index - 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;
// accum[0] /= CAF_bins_half+(d_num_doppler_bins-doppler_index); d_CAF_vector[doppler_index] += accum[0];
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 // Recompute the maximum doppler peak
volk_32f_index_max_16u_a(&indext, d_CAF_vector, d_num_doppler_bins); volk_32f_index_max_16u(&indext, d_CAF_vector, d_num_doppler_bins);
doppler=-(int)d_doppler_max+d_doppler_step*indext; doppler = -(int)d_doppler_max + d_doppler_step*indext;
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = (double)doppler;
// Dump if required, appended at the end of the file // Dump if required, appended at the end of the file
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_E5a_sat_" filename << "../data/test_statistics_E5a_sat_"
<< d_gnss_synchro->PRN << "_CAF.dat"; << d_gnss_synchro->PRN << "_CAF.dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_CAF_vector, n); d_dump_file.write((char*)d_CAF_vector, n);
d_dump_file.close(); d_dump_file.close();
} }
volk_free(accum);
} }
if (d_well_count == d_max_dwells) if (d_well_count == d_max_dwells)
@ -725,7 +698,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_state = 1; d_state = 1;
} }
consume_each(d_fft_size-d_buffer_count); consume_each(d_fft_size - d_buffer_count);
d_buffer_count = 0; d_buffer_count = 0;
break; break;
} }
@ -745,7 +718,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_active = false; d_active = false;
d_state = 0; d_state = 0;
acquisition_message = 1; acquisition_message = 1;
d_channel_internal_queue->push(acquisition_message); d_channel_internal_queue->push(acquisition_message);
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter

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

@ -79,10 +79,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_code_A = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_code_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_B = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_code_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -101,14 +100,14 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_A); volk_free(d_fft_code_A);
free(d_fft_code_B); volk_free(d_fft_code_B);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@ -121,37 +120,23 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> * code) void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
// code A: two replicas of a primary code // code A: two replicas of a primary code
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_A,d_fft_if->get_outbuf(),d_fft_size);
}
// code B: two replicas of a primary code; the second replica is inverted. // code B: two replicas of a primary code; the second replica is inverted.
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[d_samples_per_code], volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
&code[d_samples_per_code], gr_complex(-1,0), &code[d_samples_per_code], gr_complex(-1,0),
d_samples_per_code); d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_B,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_B,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void galileo_pcps_8ms_acquisition_cc::init() void galileo_pcps_8ms_acquisition_cc::init()
@ -173,12 +158,10 @@ void galileo_pcps_8ms_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){}; int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@ -241,18 +224,18 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, 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], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -262,15 +245,15 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code A reference using SIMD operations with // with the local FFT'd code A reference using SIMD operations with
// VOLK library // VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_A, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_A, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor); magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor);
@ -278,15 +261,15 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code B reference using SIMD operations with // with the local FFT'd code B reference using SIMD operations with
// VOLK library // VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_B, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_B, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor); magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor);

51
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc
View File

@ -86,9 +86,8 @@ pcps_acquisition_cc::pcps_acquisition_cc(
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
d_bit_transition_flag = bit_transition_flag; d_bit_transition_flag = bit_transition_flag;
//todo: do something if posix_memalign fails d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -107,13 +106,13 @@ pcps_acquisition_cc::~pcps_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@ -126,19 +125,9 @@ pcps_acquisition_cc::~pcps_acquisition_cc()
void pcps_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
//Conjugate the local code
if (is_unaligned())
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_acquisition_cc::init() void pcps_acquisition_cc::init()
@ -160,14 +149,12 @@ void pcps_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){}; int doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], d_freq + doppler, d_fs_in, d_fft_size);
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size);
} }
} }
@ -234,18 +221,18 @@ int pcps_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, 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], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -254,15 +241,15 @@ int pcps_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);

704
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@ -46,96 +46,94 @@
using google::LogMessage; using google::LogMessage;
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump, long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_acquisition_fine_doppler_cc_sptr( return pcps_acquisition_fine_doppler_cc_sptr(
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq, new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
fs_in, samples_per_ms, queue, dump, dump_filename)); fs_in, samples_per_ms, queue, dump, dump_filename));
} }
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump, long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
std::string dump_filename) : std::string dump_filename) :
gr::block("pcps_acquisition_fine_doppler_cc", gr::block("pcps_acquisition_fine_doppler_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(0, 0, sizeof(gr_complex))) gr::io_signature::make(0, 0, sizeof(gr_complex)))
{ {
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_queue = queue; d_queue = queue;
d_freq = freq; d_freq = freq;
d_fs_in = fs_in; d_fs_in = fs_in;
d_samples_per_ms = samples_per_ms; d_samples_per_ms = samples_per_ms;
d_sampled_ms = sampled_ms; d_sampled_ms = sampled_ms;
d_config_doppler_max = doppler_max; d_config_doppler_max = doppler_max;
d_config_doppler_min=doppler_min; d_config_doppler_min = doppler_min;
d_fft_size = d_sampled_ms * d_samples_per_ms; d_fft_size = d_sampled_ms * d_samples_per_ms;
// HS Acquisition // HS Acquisition
d_max_dwells= max_dwells; d_max_dwells = max_dwells;
d_gnuradio_forecast_samples=d_fft_size; d_gnuradio_forecast_samples = d_fft_size;
d_input_power = 0.0; d_input_power = 0.0;
d_state=0; d_state = 0;
//todo: do something if posix_memalign fails d_carrier = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_carrier, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT // Inverse FFT
d_ifft = new gr::fft::fft_complex(d_fft_size, false); d_ifft = new gr::fft::fft_complex(d_fft_size, false);
// For dumping samples into a file // For dumping samples into a file
d_dump = dump; d_dump = dump;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
} }
void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step) void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
// Create the search grid array // Create the search grid array
d_num_doppler_points=floor(std::abs(d_config_doppler_max-d_config_doppler_min)/d_doppler_step); d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step);
d_grid_data=new float*[d_num_doppler_points];
for (int i=0;i<d_num_doppler_points;i++)
{
if (posix_memalign((void**)&d_grid_data[i], 16, d_fft_size * sizeof(float)) == 0){};
}
update_carrier_wipeoff();
d_grid_data = new float*[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++)
{
d_grid_data[i] = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
}
update_carrier_wipeoff();
} }
void pcps_acquisition_fine_doppler_cc::free_grid_memory() void pcps_acquisition_fine_doppler_cc::free_grid_memory()
{ {
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
delete[] d_grid_data[i]; volk_free(d_grid_data[i]);
delete[] d_grid_doppler_wipeoffs[i]; delete[] d_grid_doppler_wipeoffs[i];
} }
delete d_grid_data; delete d_grid_data;
delete d_grid_doppler_wipeoffs; delete d_grid_doppler_wipeoffs;
} }
pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc() pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
{ {
free(d_carrier); volk_free(d_carrier);
free(d_fft_codes); volk_free(d_fft_codes);
delete d_ifft; volk_free(d_magnitude);
delete d_fft_if; delete d_ifft;
if (d_dump) delete d_fft_if;
{ if (d_dump)
d_dump_file.close(); {
} d_dump_file.close();
free_grid_memory(); }
free_grid_memory();
} }
@ -143,378 +141,370 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> * code) void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> * code)
{ {
memcpy(d_fft_if->get_inbuf(),code,sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
void pcps_acquisition_fine_doppler_cc::init() void pcps_acquisition_fine_doppler_cc::init()
{ {
d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_input_power = 0.0;
d_state=0; d_state = 0;
} }
void pcps_acquisition_fine_doppler_cc::forecast (int noutput_items, void pcps_acquisition_fine_doppler_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call
} }
void pcps_acquisition_fine_doppler_cc::reset_grid() void pcps_acquisition_fine_doppler_cc::reset_grid()
{ {
d_well_count=0; d_well_count = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i=0; i<d_num_doppler_points; i++)
{ {
for (unsigned int j=0;j<d_fft_size;j++) for (unsigned int j=0; j < d_fft_size; j++)
{ {
d_grid_data[i][j]=0.0; d_grid_data[i][j] = 0.0;
} }
} }
} }
void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
{ {
// create the carrier Doppler wipeoff signals // create the carrier Doppler wipeoff signals
int doppler_hz; int doppler_hz;
float phase_step_rad; float phase_step_rad;
d_grid_doppler_wipeoffs=new gr_complex*[d_num_doppler_points]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points];
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
doppler_hz=d_config_doppler_min+d_doppler_step*doppler_index; doppler_hz = d_config_doppler_min + d_doppler_step*doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in; phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in;
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
d_grid_doppler_wipeoffs[doppler_index]=new gr_complex[d_fft_size]; fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad);
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad); }
}
} }
double pcps_acquisition_fine_doppler_cc::search_maximum() double pcps_acquisition_fine_doppler_cc::search_maximum()
{ {
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor; float fft_normalization_factor;
int index_doppler = 0; int index_doppler = 0;
unsigned int tmp_intex_t; unsigned int tmp_intex_t;
unsigned int index_time = 0; unsigned int index_time = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i=0;i<d_num_doppler_points;i++)
{ {
volk_32f_index_max_16u_a(&tmp_intex_t,d_grid_data[i],d_fft_size); volk_32f_index_max_16u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt) if (d_grid_data[i][tmp_intex_t] > magt)
{ {
magt = d_grid_data[i][tmp_intex_t]; magt = d_grid_data[i][tmp_intex_t];
//std::cout<<magt<<std::endl; //std::cout<<magt<<std::endl;
index_doppler = i; index_doppler = i;
index_time = tmp_intex_t; index_time = tmp_intex_t;
} }
} }
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
fft_normalization_factor = (float)d_fft_size * (float)d_fft_size;; fft_normalization_factor = (float)d_fft_size * (float)d_fft_size;;
magt = magt / (fft_normalization_factor * fft_normalization_factor); magt = magt / (fft_normalization_factor * fft_normalization_factor);
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count)); d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count));
// 4- record the maximum peak and the associated synchronization parameters // 4- record the maximum peak and the associated synchronization parameters
d_gnss_synchro->Acq_delay_samples = (double)index_time; d_gnss_synchro->Acq_delay_samples = (double)index_time;
d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler*d_doppler_step+d_config_doppler_min); d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler * d_doppler_step + d_config_doppler_min);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" <<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out d_dump_file.open(filename.str().c_str(), std::ios::out
| std::ios::binary); | std::ios::binary);
d_dump_file.write((char*)d_grid_data[index_doppler], n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write((char*)d_grid_data[index_doppler], n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
return d_test_statistics; return d_test_statistics;
} }
float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items) float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items)
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 1- Compute the input signal power estimation // Compute the input signal power estimation
float power; float power = 0;
power=0; volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
if (is_unaligned()) volk_32f_accumulator_s32f(&power, d_magnitude, d_fft_size);
{
volk_32fc_magnitude_squared_32f_u(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&power, d_magnitude, d_fft_size);
}
else
{
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&power, d_magnitude, d_fft_size);
}
power /= (float)d_fft_size; power /= (float)d_fft_size;
return power; return power;
} }
int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items) int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{ {
// initialize acquisition algorithm // initialize acquisition algorithm
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN << " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_config_doppler_max << d_threshold << ", doppler_max: " << d_config_doppler_max
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector; float* p_tmp_vector = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
// doppler search steps // doppler search steps
// Perform the carrier wipe-off // Perform the carrier wipe-off
volk_32fc_x2_multiply_32fc_u(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(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal // Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute(); d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector=d_grid_data[doppler_index]; const float* old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f_u(d_grid_data[doppler_index],old_vector,p_tmp_vector,d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items, int available_samples) int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items, int available_samples)
{ {
// Direct FFT // Direct FFT
int zero_padding_factor=16; int zero_padding_factor = 16;
int fft_size_extended=d_fft_size*zero_padding_factor; int fft_size_extended = d_fft_size * zero_padding_factor;
gr::fft::fft_complex *fft_operator=new gr::fft::fft_complex(fft_size_extended,true); gr::fft::fft_complex *fft_operator = new gr::fft::fft_complex(fft_size_extended, true);
//zero padding the entire vector //zero padding the entire vector
memset(fft_operator->get_inbuf(),0,fft_size_extended*sizeof(gr_complex)); memset(fft_operator->get_inbuf(), 0, fft_size_extended * sizeof(gr_complex));
//1. generate local code aligned with the acquisition code phase estimation //1. generate local code aligned with the acquisition code phase estimation
gr_complex *code_replica; gr_complex *code_replica = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&code_replica, 16, d_fft_size * sizeof(gr_complex)) == 0){};
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
int shift_index=(int)d_gnss_synchro->Acq_delay_samples; gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
//std::cout<<"shift_index="<<shift_index<<std::endl; int shift_index = (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,code_replica+(d_fft_size-shift_index),code_replica+d_fft_size-1);
}
//2. Perform code wipe-off //std::cout<<"shift_index="<<shift_index<<std::endl;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer // Rotate to align the local code replica using acquisition time delay estimation
if (shift_index != 0)
{
std::rotate(code_replica, code_replica + (d_fft_size - shift_index), code_replica + d_fft_size - 1);
}
volk_32fc_x2_multiply_32fc_u(fft_operator->get_inbuf(), in, code_replica, d_fft_size); //2. Perform code wipe-off
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 3. Perform the FFT (zero padded!) volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
fft_operator->execute();
// 4. Compute the magnitude and find the maximum // 3. Perform the FFT (zero padded!)
float* p_tmp_vector; fft_operator->execute();
if (posix_memalign((void**)&p_tmp_vector, 16, fft_size_extended * sizeof(float)) == 0){};
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended); // 4. Compute the magnitude and find the maximum
float* p_tmp_vector = (float*)volk_malloc(fft_size_extended * sizeof(float), volk_get_alignment());
unsigned int tmp_index_freq=0; volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
volk_32f_index_max_16u_a(&tmp_index_freq,p_tmp_vector,fft_size_extended);
//std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl; unsigned int tmp_index_freq = 0;
volk_32f_index_max_16u(&tmp_index_freq,p_tmp_vector,fft_size_extended);
//case even //std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl;
int counter=0;
float fftFreqBins[fft_size_extended]; //case even
int counter = 0;
for (int k=0;k<(fft_size_extended/2);k++) float fftFreqBins[fft_size_extended];
{
fftFreqBins[counter]=(((float)d_fs_in/2.0)*(float)k)/((float)fft_size_extended/2.0);
counter++;
}
for (int k=fft_size_extended/2;k>0;k--) for (int k=0; k < (fft_size_extended / 2); k++)
{ {
fftFreqBins[counter]=((-(float)d_fs_in/2)*(float)k)/((float)fft_size_extended/2.0); fftFreqBins[counter] = (((float)d_fs_in / 2.0) * (float)k) / ((float)fft_size_extended / 2.0);
counter++; counter++;
} }
// 5. Update the Doppler estimation in Hz for (int k = fft_size_extended / 2; k > 0; k--)
if (abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<1000) {
{ fftFreqBins[counter] = ((-(float)d_fs_in / 2) * (float)k) / ((float)fft_size_extended / 2.0);
d_gnss_synchro->Acq_doppler_hz=(double)fftFreqBins[tmp_index_freq]; counter++;
//std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl; }
}else{
DLOG(INFO)<<"Abs(Grid Doppler - FFT Doppler)="<<abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<<std::endl; // 5. Update the Doppler estimation in Hz
DLOG(INFO)<<std::endl<<"Error estimating fine frequency Doppler"<<std::endl; if (abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000)
//debug log {
// d_gnss_synchro->Acq_doppler_hz = (double)fftFreqBins[tmp_index_freq];
// std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl; //std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
// std::stringstream filename; }
// std::streamsize n = sizeof(gr_complex) * (d_fft_size); else
// {
// filename.str(""); DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
// filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat"; DLOG(INFO) << "Error estimating fine frequency Doppler";
// d_dump_file.open(filename.str().c_str(), std::ios::out //debug log
// | std::ios::binary); //
// d_dump_file.write((char*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin? // std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
// d_dump_file.close(); // std::stringstream filename;
// // std::streamsize n = sizeof(gr_complex) * (d_fft_size);
// filename.str(""); //
// filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat"; // filename.str("");
// d_dump_file.open(filename.str().c_str(), std::ios::out // filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat";
// | std::ios::binary); // d_dump_file.open(filename.str().c_str(), std::ios::out
// d_dump_file.write((char*)in, n); //write directly |abs(x)|^2 in this Doppler bin? // | std::ios::binary);
// d_dump_file.close(); // d_dump_file.write((char*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin?
// // d_dump_file.close();
// //
// n = sizeof(float) * (fft_size_extended); // filename.str("");
// filename.str(""); // filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat";
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat"; // d_dump_file.open(filename.str().c_str(), std::ios::out
// d_dump_file.open(filename.str().c_str(), std::ios::out // | std::ios::binary);
// | std::ios::binary); // d_dump_file.write((char*)in, n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.write((char*)p_tmp_vector, n); //write directly |abs(x)|^2 in this Doppler bin? // d_dump_file.close();
// d_dump_file.close(); //
} //
// n = sizeof(float) * (fft_size_extended);
// filename.str("");
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat";
// d_dump_file.open(filename.str().c_str(), std::ios::out
// | std::ios::binary);
// d_dump_file.write((char*)p_tmp_vector, n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.close();
}
// free memory!! // free memory!!
delete fft_operator; delete fft_operator;
free(code_replica); volk_free(code_replica);
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
{ {
/*! /*!
* TODO: High sensitivity acquisition algorithm: * TODO: High sensitivity acquisition algorithm:
* State Mechine: * State Mechine:
* S0. StandBy. If d_active==1 -> S1 * S0. StandBy. If d_active==1 -> S1
* S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid. * S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid.
* Accumulate the search grid matrix (#doppler_bins x #fft_size) * Accumulate the search grid matrix (#doppler_bins x #fft_size)
* Compare maximum to threshold and decide positive or negative * Compare maximum to threshold and decide positive or negative
* If T>=gamma -> S4 else * If T>=gamma -> S4 else
* If d_well_count<max_dwells -> S2 * If d_well_count<max_dwells -> S2
* else -> S5. * else -> S5.
* S4. Positive_Acq: Send message and stop acq -> S0 * S4. Positive_Acq: Send message and stop acq -> S0
* S5. Negative_Acq: Send message and stop acq -> S0 * S5. Negative_Acq: Send message and stop acq -> S0
*/ */
switch (d_state) switch (d_state)
{ {
case 0: // S0. StandBy case 0: // S0. StandBy
//DLOG(INFO) <<"S0"<<std::endl; //DLOG(INFO) <<"S0"<<std::endl;
if (d_active==true) if (d_active == true)
{ {
reset_grid(); reset_grid();
d_state=1; d_state = 1;
} }
break; break;
case 1: // S1. ComputeGrid case 1: // S1. ComputeGrid
//DLOG(INFO) <<"S1"<<std::endl; //DLOG(INFO) <<"S1"<<std::endl;
compute_and_accumulate_grid(input_items); compute_and_accumulate_grid(input_items);
d_well_count++; d_well_count++;
if (d_well_count>=d_max_dwells) if (d_well_count >= d_max_dwells)
{ {
d_state=2; d_state = 2;
} }
break; break;
case 2: // Compute test statistics and decide case 2: // Compute test statistics and decide
//DLOG(INFO) <<"S2"<<std::endl; //DLOG(INFO) <<"S2"<<std::endl;
d_input_power=estimate_input_power(input_items); d_input_power = estimate_input_power(input_items);
d_test_statistics=search_maximum(); d_test_statistics = search_maximum();
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state=3; //perform fine doppler estimation d_state = 3; //perform fine doppler estimation
}else{ }
d_state=5; //negative acquisition else
} {
break; d_state = 5; //negative acquisition
}
break;
case 3: // Fine doppler estimation case 3: // Fine doppler estimation
//DLOG(INFO) <<"S3"<<std::endl; //DLOG(INFO) <<"S3"<<std::endl;
DLOG(INFO) << "Performing fine Doppler estimation"; DLOG(INFO) << "Performing fine Doppler estimation";
estimate_Doppler(input_items, ninput_items[0]); //disabled in repo estimate_Doppler(input_items, ninput_items[0]); //disabled in repo
d_state=4; d_state = 4;
break; break;
case 4: // Positive_Acq case 4: // Positive_Acq
//DLOG(INFO) <<"S4"<<std::endl; //DLOG(INFO) <<"S4"<<std::endl;
DLOG(INFO) << "positive acquisition"; DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
d_channel_internal_queue->push(1); // 1-> positive acquisition d_channel_internal_queue->push(1); // 1-> positive acquisition
d_state=0; d_state = 0;
break; break;
case 5: // Negative_Acq case 5: // Negative_Acq
//DLOG(INFO) <<"S5"<<std::endl; //DLOG(INFO) <<"S5"<<std::endl;
DLOG(INFO) << "negative acquisition"; DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
d_channel_internal_queue->push(2); // 2-> negative acquisition d_channel_internal_queue->push(2); // 2-> negative acquisition
d_state=0; d_state = 0;
break; break;
default: default:
d_state=0; d_state = 0;
break; break;
} }
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl; //DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
d_sample_counter += d_fft_size; // sample counter d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size); consume_each(d_fft_size);
return 0; return 0;
} }

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

@ -82,9 +82,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_disable_assist = false; d_disable_assist = false;
//todo: do something if posix_memalign fails d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_carrier, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_carrier = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -120,8 +119,8 @@ void pcps_assisted_acquisition_cc::free_grid_memory()
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{ {
free(d_carrier); volk_free(d_carrier);
free(d_fft_codes); volk_free(d_fft_codes);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
if (d_dump) if (d_dump)
@ -150,7 +149,7 @@ void pcps_assisted_acquisition_cc::init()
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
@ -214,7 +213,7 @@ void pcps_assisted_acquisition_cc::redefine_grid()
d_doppler_min = d_config_doppler_min; d_doppler_min = d_config_doppler_min;
} }
// Create the search grid array // Create the search grid array
d_num_doppler_points = floor(std::abs(d_doppler_max-d_doppler_min)/d_doppler_step); d_num_doppler_points = floor(std::abs(d_doppler_max - d_doppler_min) / d_doppler_step);
d_grid_data = new float*[d_num_doppler_points]; d_grid_data = new float*[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
@ -293,14 +292,14 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
float* p_tmp_vector; float* p_tmp_vector = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
volk_32fc_magnitude_squared_32f_u(p_tmp_vector, in, d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
const float* p_const_tmp_vector = p_tmp_vector; const float* p_const_tmp_vector = p_tmp_vector;
float power; float power;
volk_32f_accumulator_s32f_a(&power, p_const_tmp_vector, d_fft_size); volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
free(p_tmp_vector); volk_free(p_tmp_vector);
return ( power / (float)d_fft_size); return ( power / (float)d_fft_size);
} }
@ -319,33 +318,35 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector; float* p_tmp_vector = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
// doppler search steps // doppler search steps
// Perform the carrier wipe-off // Perform the carrier wipe-off
volk_32fc_x2_multiply_32fc_u(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(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal // Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute(); d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector = d_grid_data[doppler_index]; const float* old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f_a(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_assisted_acquisition_cc::general_work(int noutput_items, int pcps_assisted_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)

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

@ -86,14 +86,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_code_data = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_code_data, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_pilot = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_code_pilot, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_data_correlation = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_data_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_pilot_correlation = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_pilot_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_correlation_plus = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_correlation_plus, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_correlation_minus = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_correlation_minus, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -112,18 +111,18 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_data); volk_free(d_fft_code_data);
free(d_fft_code_pilot); volk_free(d_fft_code_pilot);
free(d_data_correlation); volk_free(d_data_correlation);
free(d_pilot_correlation); volk_free(d_pilot_correlation);
free(d_correlation_plus); volk_free(d_correlation_plus);
free(d_correlation_minus); volk_free(d_correlation_minus);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@ -134,8 +133,8 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
} }
} }
void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data, void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float>* code_data,
std::complex<float> * code_pilot) std::complex<float>* code_pilot)
{ {
// Data code (E1B) // Data code (E1B)
memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex)*d_fft_size);
@ -143,14 +142,7 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data,
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
}
// Pilot code (E1C) // Pilot code (E1C)
memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex)*d_fft_size);
@ -158,14 +150,7 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data,
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code, //Conjugate the local code,
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_pilot,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_pilot,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_pilot,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_cccwsr_acquisition_cc::init() void pcps_cccwsr_acquisition_cc::init()
@ -187,12 +172,11 @@ void pcps_cccwsr_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@ -252,18 +236,18 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, 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], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -273,7 +257,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd data code reference (E1B) using SIMD operations // with the local FFT'd data code reference (E1B) using SIMD operations
// with VOLK library // with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
@ -286,7 +270,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd pilot code reference (E1C) using SIMD operations // with the local FFT'd pilot code reference (E1C) using SIMD operations
// with VOLK library // with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// Compute the inverse FFT // Compute the inverse FFT
@ -307,12 +291,12 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
d_data_correlation[i].imag() - d_pilot_correlation[i].real()); d_data_correlation[i].imag() - d_pilot_correlation[i].real());
} }
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_plus, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
volk_32f_index_max_16u_a(&indext_plus, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_plus, d_magnitude, d_fft_size);
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor); magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_minus, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
volk_32f_index_max_16u_a(&indext_minus, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_minus, d_magnitude, d_fft_size);
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor); magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
if (magt_plus >= magt_minus) if (magt_plus >= magt_minus)

46
src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc
View File

@ -92,12 +92,10 @@ pcps_multithread_acquisition_cc::pcps_multithread_acquisition_cc(
//todo: do something if posix_memalign fails //todo: do something if posix_memalign fails
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
if (posix_memalign((void**)&d_in_buffer[i], 16, d_in_buffer[i] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
} }
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -116,19 +114,19 @@ pcps_multithread_acquisition_cc::~pcps_multithread_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
free(d_in_buffer[i]); volk_free(d_in_buffer[i]);
} }
delete[] d_in_buffer; delete[] d_in_buffer;
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@ -160,10 +158,9 @@ void pcps_multithread_acquisition_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@ -176,14 +173,7 @@ void pcps_multithread_acquisition_cc::set_local_code(std::complex<float> * code)
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_multithread_acquisition_cc::acquisition_core() void pcps_multithread_acquisition_cc::acquisition_core()
@ -208,18 +198,18 @@ void pcps_multithread_acquisition_cc::acquisition_core()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, 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], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -228,15 +218,15 @@ void pcps_multithread_acquisition_cc::acquisition_core()
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);

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

@ -114,17 +114,13 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_cl_fft_batch_size = 1; d_cl_fft_batch_size = 1;
d_in_buffer = new gr_complex*[d_max_dwells]; d_in_buffer = new gr_complex*[d_max_dwells];
//todo: do something if posix_memalign fails
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
if (posix_memalign((void**)&d_in_buffer[i], 16, d_in_buffer[i] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
} }
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size_pow2 * sizeof(gr_complex)) == 0){}; d_fft_codes = (gr_complex*)volk_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_zero_vector, 16, (d_fft_size_pow2-d_fft_size) * sizeof(gr_complex)) == 0){}; d_zero_vector = (gr_complex*)volk_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_get_alignment());
for (unsigned int i = 0; i < (d_fft_size_pow2-d_fft_size); i++) for (unsigned int i = 0; i < (d_fft_size_pow2-d_fft_size); i++)
{ {
@ -156,20 +152,20 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
free(d_in_buffer[i]); volk_free(d_in_buffer[i]);
} }
delete[] d_in_buffer; delete[] d_in_buffer;
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
free(d_zero_vector); volk_free(d_zero_vector);
if (d_opencl == 0) if (d_opencl == 0)
{ {
@ -318,8 +314,7 @@ void pcps_opencl_acquisition_cc::init()
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler= -(int)d_doppler_max + d_doppler_step*doppler_index; int doppler= -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
@ -347,44 +342,37 @@ void pcps_opencl_acquisition_cc::init()
void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
if(d_opencl == 0) if(d_opencl == 0)
{ {
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0,
sizeof(gr_complex)*d_fft_size, code); sizeof(gr_complex)*d_fft_size, code);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)*d_fft_size, 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), sizeof(gr_complex)*(d_fft_size_pow2 - 2*d_fft_size),
d_zero_vector); d_zero_vector);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)
*(d_fft_size_pow2 - d_fft_size), *(d_fft_size_pow2 - d_fft_size),
sizeof(gr_complex)*d_fft_size, code); sizeof(gr_complex)*d_fft_size, code);
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size, clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(), clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
0, NULL, NULL); 0, NULL, NULL);
//Conjucate the local code //Conjucate the local code
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector"); cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
kernel.setArg(0, *d_cl_buffer_2); //input kernel.setArg(0, *d_cl_buffer_2); //input
kernel.setArg(1, *d_cl_buffer_fft_codes); //output kernel.setArg(1, *d_cl_buffer_fft_codes); //output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange); d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
} }
else else
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{ }
volk_32fc_conjugate_32fc_u(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
}
} }
void pcps_opencl_acquisition_cc::acquisition_core_volk() void pcps_opencl_acquisition_cc::acquisition_core_volk()
@ -409,19 +397,17 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -430,15 +416,15 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@ -543,8 +529,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
cl::Kernel kernel; cl::Kernel kernel;
@ -600,7 +586,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// Search maximum // Search maximum
// @TODO: find an efficient way to search the maximum with OpenCL in the GPU. // @TODO: find an efficient way to search the maximum with OpenCL in the GPU.
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);

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

@ -70,9 +70,9 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename): std::string dump_filename):
gr::block("pcps_quicksync_acquisition_cc", gr::block("pcps_quicksync_acquisition_cc",
gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )), gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )),
gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms ))) gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms )))
{ {
//DLOG(INFO) << "START CONSTRUCTOR"; //DLOG(INFO) << "START CONSTRUCTOR";
@ -97,20 +97,17 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
//fft size is reduced. //fft size is reduced.
d_fft_size = (d_samples_per_code) / d_folding_factor; d_fft_size = (d_samples_per_code) / d_folding_factor;
d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
//todo: do something if posix_memalign fails d_magnitude = (float*)volk_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude_folded = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_samples_per_code * d_folding_factor * sizeof(float)) == 0){};
if (posix_memalign((void**)&d_magnitude_folded, 16, d_fft_size * sizeof(float)) == 0){};
d_possible_delay = new unsigned int[d_folding_factor]; d_possible_delay = new unsigned int[d_folding_factor];
d_corr_output_f = new float[d_folding_factor]; d_corr_output_f = new float[d_folding_factor];
/*Create the d_code signal , which would store the values of the code in its /*Create the d_code signal , which would store the values of the code in its
original form to perform later correlation in time domain*/ original form to perform later correlation in time domain*/
d_code = new gr_complex[d_samples_per_code](); d_code = new gr_complex[d_samples_per_code]();
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT // Inverse FFT
@ -130,25 +127,20 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
free(d_magnitude_folded); volk_free(d_magnitude_folded);
delete d_ifft; delete d_ifft;
d_ifft = NULL;
delete d_fft_if; delete d_fft_if;
d_fft_if = NULL;
delete d_code; delete d_code;
d_code = NULL;
delete d_possible_delay; delete d_possible_delay;
d_possible_delay = NULL;
delete d_corr_output_f; delete d_corr_output_f;
d_corr_output_f = NULL;
if (d_dump) if (d_dump)
{ {
d_dump_file.close(); d_dump_file.close();
@ -156,11 +148,9 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
// DLOG(INFO) << "END DESTROYER"; // DLOG(INFO) << "END DESTROYER";
} }
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
// DLOG(INFO) << "START LOCAL CODE";
/*save a local copy of the code without the folding process to perform corre- /*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*/ lation in time in the final steps of the acquisition stage*/
memcpy(d_code, code, sizeof(gr_complex)*d_samples_per_code); memcpy(d_code, code, sizeof(gr_complex)*d_samples_per_code);
@ -182,16 +172,7 @@ void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code)
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
// DLOG(INFO) << "END LOCAL CODE";
} }
void pcps_quicksync_acquisition_cc::init() void pcps_quicksync_acquisition_cc::init()
@ -216,10 +197,8 @@ void pcps_quicksync_acquisition_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_get_alignment());
d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){}; int doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_freq + doppler, d_fs_in,
d_samples_per_code * d_folding_factor); d_samples_per_code * d_folding_factor);
@ -278,22 +257,16 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
float magt = 0.0; float magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
gr_complex *in_temp; gr_complex *in_temp = (gr_complex*)volk_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&(in_temp), 16,d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){}; gr_complex *in_temp_folded = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
gr_complex *in_temp_folded;
if (posix_memalign((void**)&(in_temp_folded), 16,d_fft_size * sizeof(gr_complex)) == 0){};
/*Create a signal to store a signal of size 1ms, to perform correlation /*Create a signal to store a signal of size 1ms, to perform correlation
in time. No folding on this data is required*/ in time. No folding on this data is required*/
gr_complex *in_1code; gr_complex *in_1code = (gr_complex*)volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&(in_1code), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores the values of the correlation output between the local code /*Stores the values of the correlation output between the local code
and the signal with doppler shift corrected */ and the signal with doppler shift corrected */
gr_complex *corr_output; gr_complex *corr_output = (gr_complex*)volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&(corr_output), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores a copy of the folded version of the signal.This is used for /*Stores a copy of the folded version of the signal.This is used for
the FFT operations in future steps of excecution*/ the FFT operations in future steps of excecution*/
@ -322,30 +295,27 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/* 1- Compute the input signal power estimation. This operation is /* 1- Compute the input signal power estimation. This operation is
being performed in a signal of size nxp */ being performed in a signal of size nxp */
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_samples_per_code * d_folding_factor); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
d_input_power /= (float)(d_samples_per_code * d_folding_factor); d_input_power /= (float)(d_samples_per_code * d_folding_factor);
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++)
{ {
/*Ensure that the signal is going to start with all samples /*Ensure that the signal is going to start with all samples
at zero. This is done to avoid over acumulation when performing at zero. This is done to avoid over acumulation when performing
the folding process to be stored in d_fft_if->get_inbuf()*/ the folding process to be stored in d_fft_if->get_inbuf()*/
d_signal_folded = new gr_complex[d_fft_size](); d_signal_folded = new gr_complex[d_fft_size]();
memcpy( d_fft_if->get_inbuf(),d_signal_folded, memcpy( d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
sizeof(gr_complex)*(d_fft_size));
/*Doppler search steps and then multiplication of the incoming /*Doppler search steps and then multiplication of the incoming
signal with the doppler wipeoffs to eliminate frequency offset signal with the doppler wipeoffs to eliminate frequency offset
*/ */
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
/*Perform multiplication of the incoming signal with the /*Perform multiplication of the incoming signal with the
complex exponential vector. This removes the frequency doppler complex exponential vector. This removes the frequency doppler
shift offset*/ shift offset*/
volk_32fc_x2_multiply_32fc_a(in_temp, in, volk_32fc_x2_multiply_32fc(in_temp, in,
d_grid_doppler_wipeoffs[doppler_index], d_grid_doppler_wipeoffs[doppler_index],
d_samples_per_code * d_folding_factor); d_samples_per_code * d_folding_factor);
@ -354,8 +324,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
incoming raw data signal is of d_folding_factor^2*/ incoming raw data signal is of d_folding_factor^2*/
for ( int i = 0; i < (int)(d_folding_factor*d_folding_factor); i++) for ( int i = 0; i < (int)(d_folding_factor*d_folding_factor); i++)
{ {
std::transform ((in_temp+i*d_fft_size), std::transform ((in_temp + i * d_fft_size),
(in_temp+((i+1)*d_fft_size)) , (in_temp + ((i + 1) * d_fft_size)) ,
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>()); std::plus<gr_complex>());
@ -368,7 +338,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/*Multiply carrier wiped--off, Fourier transformed incoming /*Multiply carrier wiped--off, Fourier transformed incoming
signal with the local FFT'd code reference using SIMD signal with the local FFT'd code reference using SIMD
operations with VOLK library*/ operations with VOLK library*/
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
/* compute the inverse FFT of the aliased signal*/ /* compute the inverse FFT of the aliased signal*/
@ -376,14 +346,14 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/* Compute the magnitude and get the maximum value with its /* Compute the magnitude and get the maximum value with its
index position*/ index position*/
volk_32fc_magnitude_squared_32f_a(d_magnitude_folded, volk_32fc_magnitude_squared_32f(d_magnitude_folded,
d_ifft->get_outbuf(), d_fft_size); d_ifft->get_outbuf(), d_fft_size);
/* Normalize the maximum value to correct the scale factor /* Normalize the maximum value to correct the scale factor
introduced by FFTW*/ introduced by FFTW*/
//volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded, //volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded,
// (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size); // (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude_folded, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude_folded, d_fft_size);
magt = d_magnitude_folded[indext]/ (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude_folded[indext]/ (fft_normalization_factor * fft_normalization_factor);
@ -415,7 +385,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
for (int i = 0; i < (int)d_folding_factor; i++) for (int i = 0; i < (int)d_folding_factor; i++)
{ {
d_possible_delay[i]= detected_delay_samples_folded+ d_possible_delay[i] = detected_delay_samples_folded +
(i)*d_fft_size; (i)*d_fft_size;
} }
@ -432,61 +402,42 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
effect corrected and accumulates its value. This effect corrected and accumulates its value. This
is indeed correlation in time for an specific value is indeed correlation in time for an specific value
of a shift*/ of a shift*/
volk_32fc_x2_multiply_32fc_a(corr_output, in_1code, volk_32fc_x2_multiply_32fc(corr_output, in_1code,
d_code, d_samples_per_code); d_code, d_samples_per_code);
for(int j=0; j < (d_samples_per_code); j++) for(int j = 0; j < d_samples_per_code; j++)
{ {
complex_acumulator[i] += (corr_output[j]); complex_acumulator[i] += (corr_output[j]);
} }
} }
/*Obtain maximun value of correlation given the /*Obtain maximun value of correlation given the possible delay selected */
possible delay selected */ volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator, d_folding_factor);
volk_32fc_magnitude_squared_32f_a(d_corr_output_f, volk_32f_index_max_16u(&indext, d_corr_output_f, d_folding_factor);
complex_acumulator, d_folding_factor);
volk_32f_index_max_16u_a(&indext, d_corr_output_f,
d_folding_factor);
/*Now save the real code phase in the gnss_syncro /*Now save the real code phase in the gnss_syncro block for use in other stages*/
block for use in other stages*/ d_gnss_synchro->Acq_delay_samples = (double)(d_possible_delay[indext]);
d_gnss_synchro->Acq_delay_samples = (double)
(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = (double)doppler;
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
/* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
/* 5- Compute the test statistics and compare to the threshold
d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
d_test_statistics = d_mag / d_input_power; d_test_statistics = d_mag / d_input_power;
//delete complex_acumulator; //delete complex_acumulator;
} }
} }
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
/*
std::stringstream filename;
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->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
*/
/*Since QuickSYnc performs a folded correlation in frequency by means /*Since QuickSYnc performs a folded correlation in frequency by means
of the FFT, it is esential to also keep the values obtained from the of the FFT, it is esential to also keep the values obtained from the
possible delay to show how it is maximize*/ possible delay to show how it is maximize*/
std::stringstream filename; std::stringstream filename;
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" <<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write((char*)d_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
@ -524,11 +475,9 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
consume_each(1); consume_each(1);
delete d_code_folded; delete d_code_folded;
d_code_folded = NULL; volk_free(in_temp);
volk_free(in_1code);
free(in_temp); volk_free(corr_output);
free(in_1code);
free(corr_output);
break; break;
} }

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

@ -99,9 +99,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_codes = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -120,15 +119,15 @@ pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
free(d_grid_data[i]); volk_free(d_grid_data[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
delete[] d_grid_data; delete[] d_grid_data;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@ -146,14 +145,7 @@ void pcps_tong_acquisition_cc::set_local_code(std::complex<float> * code)
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
} }
void pcps_tong_acquisition_cc::init() void pcps_tong_acquisition_cc::init()
@ -178,16 +170,14 @@ void pcps_tong_acquisition_cc::init()
d_grid_data = new float*[d_num_doppler_bins]; d_grid_data = new float*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = (gr_complex*)volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment());
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
if (posix_memalign((void**)&(d_grid_data[doppler_index]), 16, d_grid_data[doppler_index] = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
d_fft_size * sizeof(float)) == 0){};
for (unsigned int i = 0; i < d_fft_size; i++) for (unsigned int i = 0; i < d_fft_size; i++)
{ {
@ -257,8 +247,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
@ -268,7 +258,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, 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], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@ -277,25 +267,25 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), 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, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Compute magnitude // Compute magnitude
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
// Compute vector of test statistics corresponding to current doppler index. // Compute vector of test statistics corresponding to current doppler index.
volk_32f_s32f_multiply_32f_a(d_magnitude, d_magnitude, volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
1/(fft_normalization_factor*fft_normalization_factor*d_input_power), 1/(fft_normalization_factor*fft_normalization_factor*d_input_power),
d_fft_size); d_fft_size);
// Accumulate test statistics in d_grid_data. // Accumulate test statistics in d_grid_data.
volk_32f_x2_add_32f_a(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], d_magnitude, d_grid_data[doppler_index], d_fft_size);
// Search maximum // Search maximum
volk_32f_index_max_16u_a(&indext, d_grid_data[doppler_index], d_fft_size); volk_32f_index_max_16u(&indext, d_grid_data[doppler_index], d_fft_size);
magt = d_grid_data[doppler_index][indext]; magt = d_grid_data[doppler_index][indext];

1
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
View File

@ -191,7 +191,6 @@ Galileo_E5a_Dll_Pll_Tracking_cc::~Galileo_E5a_Dll_Pll_Tracking_cc ()
volk_free(d_Early); volk_free(d_Early);
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
volk_free(d_Prompt_data);
volk_free(d_codeQ); volk_free(d_codeQ);
volk_free(d_codeI); volk_free(d_codeI);