mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-10 20:10:05 +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

152
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,14 +198,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code 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)
{ {
@ -214,51 +209,32 @@ 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_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 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_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) if (d_both_signal_components == true)
{ {
// PILOT CODE B: First replica is inverted (0,1,1) // PILOT 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],
&codeQ[0], gr_complex(-1,0), &codeQ[0], 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_Q_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);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size);
}
} }
} }
} }
@ -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,8 +382,8 @@ 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
@ -419,7 +393,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_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(), 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);
} }
@ -610,9 +584,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
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;
if (posix_memalign((void**)&accum, 16, sizeof(float)) == 0){};
CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step); CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step);
float weighting_factor; float weighting_factor;
weighting_factor = 0.5/(float)CAF_bins_half; weighting_factor = 0.5/(float)CAF_bins_half;
@ -692,7 +664,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
} }
// 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
@ -707,6 +679,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
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)
@ -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

57
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;
@ -127,31 +126,17 @@ void galileo_pcps_8ms_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_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()
@ -175,9 +160,7 @@ void galileo_pcps_8ms_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);
@ -241,8 +224,8 @@ 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
@ -252,7 +235,7 @@ int galileo_pcps_8ms_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)
@ -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);

43
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;
@ -127,18 +126,8 @@ 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; 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);
} }
} }
@ -234,8 +221,8 @@ 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
@ -245,7 +232,7 @@ int pcps_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)
@ -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);

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

@ -81,10 +81,9 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
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);
@ -107,28 +106,27 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste
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++)
{ {
if (posix_memalign((void**)&d_grid_data[i], 16, d_fft_size * sizeof(float)) == 0){}; d_grid_data[i] = (float*)volk_malloc(d_fft_size * sizeof(float), volk_get_alignment());
} }
update_carrier_wipeoff(); 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);
volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
if (d_dump) if (d_dump)
@ -146,8 +144,7 @@ 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()
@ -191,7 +188,6 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
// 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);
} }
@ -207,7 +203,7 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
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];
@ -250,22 +246,12 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
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)
@ -282,34 +268,33 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
// 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;
} }
@ -324,8 +309,8 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
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); 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; int shift_index = (int)d_gnss_synchro->Acq_delay_samples;
@ -340,19 +325,18 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
//2. Perform code wipe-off //2. Perform code wipe-off
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
volk_32fc_x2_multiply_32fc_u(fft_operator->get_inbuf(), in, code_replica, d_fft_size); volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
// 3. Perform the FFT (zero padded!) // 3. Perform the FFT (zero padded!)
fft_operator->execute(); fft_operator->execute();
// 4. Compute the magnitude and find the maximum // 4. Compute the magnitude and find the maximum
float* p_tmp_vector; float* p_tmp_vector = (float*)volk_malloc(fft_size_extended * sizeof(float), volk_get_alignment());
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); volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
unsigned int tmp_index_freq = 0; unsigned int tmp_index_freq = 0;
volk_32f_index_max_16u_a(&tmp_index_freq,p_tmp_vector,fft_size_extended); volk_32f_index_max_16u(&tmp_index_freq,p_tmp_vector,fft_size_extended);
//std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl; //std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl;
@ -378,9 +362,11 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
{ {
d_gnss_synchro->Acq_doppler_hz = (double)fftFreqBins[tmp_index_freq]; 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;
}else{ }
DLOG(INFO)<<"Abs(Grid Doppler - FFT Doppler)="<<abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<<std::endl; else
DLOG(INFO)<<std::endl<<"Error estimating fine frequency Doppler"<<std::endl; {
DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
DLOG(INFO) << "Error estimating fine frequency Doppler";
//debug log //debug log
// //
// 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;
@ -414,10 +400,12 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
// 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)
@ -463,7 +451,9 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
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{ }
else
{
d_state = 5; //negative acquisition d_state = 5; //negative acquisition
} }
break; break;

37
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);
} }
@ -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)

70
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;
@ -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()
@ -189,8 +174,7 @@ void pcps_cccwsr_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],
@ -252,8 +236,8 @@ 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
@ -263,7 +247,7 @@ int pcps_cccwsr_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)
@ -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)

40
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,8 +158,7 @@ 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],
@ -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,8 +198,8 @@ 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
@ -219,7 +209,7 @@ void pcps_multithread_acquisition_cc::acquisition_core()
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);

54
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],
@ -376,14 +371,7 @@ void pcps_opencl_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);
}
} }
} }
@ -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);

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

@ -97,11 +97,9 @@ 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];
@ -110,7 +108,6 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
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,9 +197,7 @@ 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,
@ -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,20 +295,17 @@ 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
@ -345,7 +315,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/*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);
@ -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);
@ -432,52 +402,33 @@ 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*/
@ -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;
} }

44
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);