mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Switch to automatically managed static memory
This commit is contained in:
parent
31b6f9defd
commit
05faf97b70
@ -106,42 +106,26 @@ 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_;
|
||||||
|
|
||||||
d_inbuffer = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_inbuffer.reserve(d_fft_size);
|
||||||
d_fft_code_I_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_I_A.reserve(d_fft_size);
|
||||||
d_magnitudeIA = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitudeIA.reserve(d_fft_size);
|
||||||
|
|
||||||
if (d_both_signal_components == true)
|
if (d_both_signal_components == true)
|
||||||
{
|
{
|
||||||
d_fft_code_Q_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_Q_A.reserve(d_fft_size);
|
||||||
d_magnitudeQA = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitudeQA.reserve(d_fft_size);
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
d_fft_code_Q_A = nullptr;
|
|
||||||
d_magnitudeQA = nullptr;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// IF COHERENT INTEGRATION TIME > 1
|
// IF COHERENT INTEGRATION TIME > 1
|
||||||
if (d_sampled_ms > 1)
|
if (d_sampled_ms > 1)
|
||||||
{
|
{
|
||||||
d_fft_code_I_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_I_B.reserve(d_fft_size);
|
||||||
d_magnitudeIB = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitudeIB.reserve(d_fft_size);
|
||||||
if (d_both_signal_components == true)
|
if (d_both_signal_components == true)
|
||||||
{
|
{
|
||||||
d_fft_code_Q_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_Q_B.reserve(d_fft_size);
|
||||||
d_magnitudeQB = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitudeQB.reserve(d_fft_size);
|
||||||
}
|
}
|
||||||
else
|
|
||||||
{
|
|
||||||
d_fft_code_Q_B = nullptr;
|
|
||||||
d_magnitudeQB = nullptr;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
d_fft_code_I_B = nullptr;
|
|
||||||
d_magnitudeIB = nullptr;
|
|
||||||
d_fft_code_Q_B = nullptr;
|
|
||||||
d_magnitudeQB = nullptr;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
@ -157,14 +141,10 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
|
|||||||
d_doppler_resolution = 0;
|
d_doppler_resolution = 0;
|
||||||
d_threshold = 0;
|
d_threshold = 0;
|
||||||
d_doppler_step = 250;
|
d_doppler_step = 250;
|
||||||
d_grid_doppler_wipeoffs = nullptr;
|
|
||||||
d_gnss_synchro = nullptr;
|
d_gnss_synchro = nullptr;
|
||||||
d_code_phase = 0;
|
d_code_phase = 0;
|
||||||
d_doppler_freq = 0;
|
d_doppler_freq = 0;
|
||||||
d_test_statistics = 0;
|
d_test_statistics = 0;
|
||||||
d_CAF_vector = nullptr;
|
|
||||||
d_CAF_vector_I = nullptr;
|
|
||||||
d_CAF_vector_Q = nullptr;
|
|
||||||
d_channel = 0;
|
d_channel = 0;
|
||||||
d_gr_stream_buffer = 0;
|
d_gr_stream_buffer = 0;
|
||||||
}
|
}
|
||||||
@ -172,44 +152,6 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
|
|||||||
|
|
||||||
galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisition_caf_cc()
|
galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisition_caf_cc()
|
||||||
{
|
{
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
}
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_inbuffer);
|
|
||||||
volk_gnsssdr_free(d_fft_code_I_A);
|
|
||||||
volk_gnsssdr_free(d_magnitudeIA);
|
|
||||||
if (d_both_signal_components == true)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_fft_code_Q_A);
|
|
||||||
volk_gnsssdr_free(d_magnitudeQA);
|
|
||||||
}
|
|
||||||
// IF INTEGRATION TIME > 1
|
|
||||||
if (d_sampled_ms > 1)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_fft_code_I_B);
|
|
||||||
volk_gnsssdr_free(d_magnitudeIB);
|
|
||||||
if (d_both_signal_components == true)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_fft_code_Q_B);
|
|
||||||
volk_gnsssdr_free(d_magnitudeQB);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (d_CAF_window_hz > 0)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_CAF_vector);
|
|
||||||
volk_gnsssdr_free(d_CAF_vector_I);
|
|
||||||
if (d_both_signal_components == true)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_CAF_vector_Q);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -236,8 +178,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_I_A, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_I_A.data(), 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)
|
||||||
@ -247,8 +189,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_Q_A, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_Q_A.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
// IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination
|
// 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)
|
||||||
@ -261,8 +203,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_I_B, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_I_B.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
|
|
||||||
if (d_both_signal_components == true)
|
if (d_both_signal_components == true)
|
||||||
{
|
{
|
||||||
@ -272,8 +214,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
|
|||||||
d_samples_per_code);
|
d_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
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_Q_B, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_Q_B.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -304,26 +246,24 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Create the carrier Doppler wipeoff signals
|
// Create the carrier Doppler wipeoff signals
|
||||||
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
|
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
|
||||||
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = GALILEO_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = GALILEO_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* CAF Filtering to resolve doppler ambiguity. Phase and quadrature must be processed
|
/* CAF Filtering to resolve doppler ambiguity. Phase and quadrature must be processed
|
||||||
* separately before non-coherent integration */
|
* separately before non-coherent integration */
|
||||||
// if (d_CAF_filter)
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_CAF_vector.reserve(d_num_doppler_bins);
|
||||||
d_CAF_vector_I = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_CAF_vector_I.reserve(d_num_doppler_bins);
|
||||||
if (d_both_signal_components == true)
|
if (d_both_signal_components == true)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q = static_cast<float *>(volk_gnsssdr_malloc(d_num_doppler_bins * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_CAF_vector_Q.reserve(d_num_doppler_bins);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -369,7 +309,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
* 7. Declare positive or negative acquisition using a message port
|
* 7. Declare positive or negative acquisition using a message port
|
||||||
*/
|
*/
|
||||||
|
|
||||||
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
/* States: 0 Stop Channel
|
/* States: 0 Stop Channel
|
||||||
* 1 Load the buffer until it reaches fft_size
|
* 1 Load the buffer until it reaches fft_size
|
||||||
* 2 Acquisition algorithm
|
* 2 Acquisition algorithm
|
||||||
@ -382,7 +322,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
{
|
{
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -400,7 +340,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
}
|
}
|
||||||
case 1:
|
case 1:
|
||||||
{
|
{
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
unsigned int buff_increment;
|
unsigned int buff_increment;
|
||||||
if ((ninput_items[0] + d_buffer_count) <= d_fft_size)
|
if ((ninput_items[0] + d_buffer_count) <= d_fft_size)
|
||||||
{
|
{
|
||||||
@ -424,7 +364,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
case 2:
|
case 2:
|
||||||
{
|
{
|
||||||
// Fill last part of the buffer and reset counter
|
// Fill last part of the buffer and reset counter
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
if (d_buffer_count < d_fft_size)
|
if (d_buffer_count < d_fft_size)
|
||||||
{
|
{
|
||||||
memcpy(&d_inbuffer[d_buffer_count], in, sizeof(gr_complex) * (d_fft_size - d_buffer_count));
|
memcpy(&d_inbuffer[d_buffer_count], in, sizeof(gr_complex) * (d_fft_size - d_buffer_count));
|
||||||
@ -455,19 +395,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
<< ", doppler_step: " << d_doppler_step;
|
<< ", doppler_step: " << d_doppler_step;
|
||||||
|
|
||||||
// 1- Compute the input signal power estimation
|
// 1- Compute the input signal power estimation
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_inbuffer, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitudeIA.data(), d_inbuffer.data(), d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<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 = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
|
|
||||||
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer,
|
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer.data(),
|
||||||
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
|
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
|
||||||
|
|
||||||
// 3- Perform the FFT-based convolution (parallel time search)
|
// 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
|
||||||
@ -477,14 +416,14 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_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(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.data(), 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(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitudeIA.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_IA, d_magnitudeIA, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_IA, d_magnitudeIA.data(), d_fft_size);
|
||||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
// 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);
|
||||||
|
|
||||||
@ -492,30 +431,30 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
{
|
{
|
||||||
// REPEAT FOR ALL CODES. CODE_QA
|
// REPEAT FOR ALL CODES. CODE_QA
|
||||||
volk_32fc_x2_multiply_32fc(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.data(), d_fft_size);
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitudeQA.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_QA, d_magnitudeQA, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_QA, d_magnitudeQA.data(), d_fft_size);
|
||||||
magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor);
|
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(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.data(), d_fft_size);
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitudeIB.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_IB, d_magnitudeIB, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_IB, d_magnitudeIB.data(), d_fft_size);
|
||||||
magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor);
|
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(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.data(), d_fft_size);
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitudeQB.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_QB, d_magnitudeQB, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_QB, d_magnitudeQB.data(), d_fft_size);
|
||||||
magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor);
|
magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -528,7 +467,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
{
|
{
|
||||||
if (magt_IA >= magt_IB)
|
if (magt_IA >= magt_IB)
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IA;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
|
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
|
||||||
@ -538,7 +476,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
// Integrate non-coherently I+Q
|
// Integrate non-coherently I+Q
|
||||||
if (magt_QA >= magt_QB)
|
if (magt_QA >= magt_QB)
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
||||||
@ -550,7 +487,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QB;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
|
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
|
||||||
@ -561,12 +497,11 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA.data(), d_fft_size);
|
||||||
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
|
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IB;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_I[doppler_index] = d_magnitudeIB[indext_IB];
|
d_CAF_vector_I[doppler_index] = d_magnitudeIB[indext_IB];
|
||||||
@ -576,7 +511,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
// Integrate non-coherently I+Q
|
// Integrate non-coherently I+Q
|
||||||
if (magt_QA >= magt_QB)
|
if (magt_QA >= magt_QB)
|
||||||
{
|
{
|
||||||
//if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
||||||
@ -588,7 +522,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QB;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
|
d_CAF_vector_Q[doppler_index] = d_magnitudeQB[indext_QB];
|
||||||
@ -599,20 +532,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIB, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIB.data(), d_fft_size);
|
||||||
magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor);
|
magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_I[doppler_index] = magt_IA;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
|
d_CAF_vector_I[doppler_index] = d_magnitudeIA[indext_IA];
|
||||||
}
|
}
|
||||||
if (d_both_signal_components)
|
if (d_both_signal_components)
|
||||||
{
|
{
|
||||||
// if (d_CAF_filter) {d_CAF_vector_Q[doppler_index] = magt_QA;}
|
|
||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
d_CAF_vector_Q[doppler_index] = d_magnitudeQA[indext_QA];
|
||||||
@ -623,7 +554,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
d_magnitudeIA[i] += d_magnitudeQA[i];
|
d_magnitudeIA[i] += d_magnitudeQA[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitudeIA.data(), d_fft_size);
|
||||||
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
|
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -662,16 +593,16 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
{
|
{
|
||||||
if (magt_IA >= magt_IB)
|
if (magt_IA >= magt_IB)
|
||||||
{
|
{
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA), n);
|
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA.data()), n);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIB), n);
|
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIB.data()), n);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA), n);
|
d_dump_file.write(reinterpret_cast<char *>(d_magnitudeIA.data()), n);
|
||||||
}
|
}
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
@ -681,7 +612,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
if (d_CAF_window_hz > 0)
|
if (d_CAF_window_hz > 0)
|
||||||
{
|
{
|
||||||
int CAF_bins_half;
|
int CAF_bins_half;
|
||||||
auto *accum = static_cast<float *>(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment()));
|
std::array<float, 1> accum{};
|
||||||
CAF_bins_half = d_CAF_window_hz / (2 * d_doppler_step);
|
CAF_bins_half = d_CAF_window_hz / (2 * d_doppler_step);
|
||||||
float weighting_factor;
|
float weighting_factor;
|
||||||
weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
|
weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
|
||||||
@ -691,22 +622,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
for (int doppler_index = 0; doppler_index < CAF_bins_half; doppler_index++)
|
for (int doppler_index = 0; doppler_index < CAF_bins_half; doppler_index++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] = 0;
|
d_CAF_vector[doppler_index] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], d_CAF_vector_I, CAF_bins_half+doppler_index+1);
|
|
||||||
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
|
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
||||||
}
|
}
|
||||||
// d_CAF_vector[doppler_index] /= CAF_bins_half+doppler_index+1;
|
|
||||||
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
|
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
|
||||||
if (d_both_signal_components)
|
if (d_both_signal_components)
|
||||||
{
|
{
|
||||||
accum[0] = 0;
|
accum[0] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&accum[0], d_CAF_vector_Q, CAF_bins_half+doppler_index+1);
|
|
||||||
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
|
for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
|
||||||
{
|
{
|
||||||
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
|
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
|
||||||
}
|
}
|
||||||
// accum[0] /= CAF_bins_half+doppler_index+1;
|
|
||||||
accum[0] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
|
accum[0] /= 1 + CAF_bins_half + doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
|
||||||
d_CAF_vector[doppler_index] += accum[0];
|
d_CAF_vector[doppler_index] += accum[0];
|
||||||
}
|
}
|
||||||
@ -715,22 +642,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
for (unsigned int doppler_index = CAF_bins_half; doppler_index < d_num_doppler_bins - CAF_bins_half; doppler_index++)
|
for (unsigned int doppler_index = CAF_bins_half; doppler_index < d_num_doppler_bins - CAF_bins_half; doppler_index++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] = 0;
|
d_CAF_vector[doppler_index] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], 2*CAF_bins_half+1);
|
|
||||||
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
|
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
||||||
}
|
}
|
||||||
// d_CAF_vector[doppler_index] /= 2*CAF_bins_half+1;
|
|
||||||
d_CAF_vector[doppler_index] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
|
d_CAF_vector[doppler_index] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
|
||||||
if (d_both_signal_components)
|
if (d_both_signal_components)
|
||||||
{
|
{
|
||||||
accum[0] = 0;
|
accum[0] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], 2*CAF_bins_half);
|
|
||||||
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
|
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(doppler_index + CAF_bins_half + 1); i++)
|
||||||
{
|
{
|
||||||
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>((doppler_index - i)));
|
||||||
}
|
}
|
||||||
// accum[0] /= 2*CAF_bins_half+1;
|
|
||||||
accum[0] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
|
accum[0] /= 1 + 2 * CAF_bins_half - 2 * weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
|
||||||
d_CAF_vector[doppler_index] += accum[0];
|
d_CAF_vector[doppler_index] += accum[0];
|
||||||
}
|
}
|
||||||
@ -739,29 +662,25 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
for (int doppler_index = d_num_doppler_bins - CAF_bins_half; doppler_index < static_cast<int>(d_num_doppler_bins); doppler_index++)
|
for (int doppler_index = d_num_doppler_bins - CAF_bins_half; doppler_index < static_cast<int>(d_num_doppler_bins); doppler_index++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] = 0;
|
d_CAF_vector[doppler_index] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
|
|
||||||
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
|
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
|
||||||
{
|
{
|
||||||
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * (abs(doppler_index - i)));
|
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * (abs(doppler_index - i)));
|
||||||
}
|
}
|
||||||
// d_CAF_vector[doppler_index] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
|
|
||||||
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
|
d_CAF_vector[doppler_index] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
|
||||||
if (d_both_signal_components)
|
if (d_both_signal_components)
|
||||||
{
|
{
|
||||||
accum[0] = 0;
|
accum[0] = 0;
|
||||||
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
|
|
||||||
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
|
for (int i = doppler_index - CAF_bins_half; i < static_cast<int>(d_num_doppler_bins); i++)
|
||||||
{
|
{
|
||||||
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * (abs(doppler_index - i)));
|
accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * (abs(doppler_index - i)));
|
||||||
}
|
}
|
||||||
// accum[0] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
|
|
||||||
accum[0] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
|
accum[0] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
|
||||||
d_CAF_vector[doppler_index] += accum[0];
|
d_CAF_vector[doppler_index] += accum[0];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Recompute the maximum doppler peak
|
// Recompute the maximum doppler peak
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_CAF_vector, d_num_doppler_bins);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_CAF_vector.data(), d_num_doppler_bins);
|
||||||
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * indext;
|
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * indext;
|
||||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||||
// Dump if required, appended at the end of the file
|
// Dump if required, appended at the end of the file
|
||||||
@ -772,10 +691,9 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
|||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_E5a_sat_" << d_gnss_synchro->PRN << "_CAF.dat";
|
filename << "../data/test_statistics_E5a_sat_" << d_gnss_synchro->PRN << "_CAF.dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_CAF_vector), n);
|
d_dump_file.write(reinterpret_cast<char *>(d_CAF_vector.data()), n);
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
volk_gnsssdr_free(accum);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (d_well_count == d_max_dwells)
|
if (d_well_count == d_max_dwells)
|
||||||
|
@ -47,6 +47,7 @@
|
|||||||
#include <memory>
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
class galileo_e5a_noncoherentIQ_acquisition_caf_cc;
|
class galileo_e5a_noncoherentIQ_acquisition_caf_cc;
|
||||||
|
|
||||||
@ -221,23 +222,23 @@ private:
|
|||||||
unsigned int d_well_count;
|
unsigned int d_well_count;
|
||||||
unsigned int d_fft_size;
|
unsigned int d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
unsigned int d_num_doppler_bins;
|
unsigned int d_num_doppler_bins;
|
||||||
gr_complex* d_fft_code_I_A;
|
std::vector<gr_complex> d_fft_code_I_A;
|
||||||
gr_complex* d_fft_code_I_B;
|
std::vector<gr_complex> d_fft_code_I_B;
|
||||||
gr_complex* d_fft_code_Q_A;
|
std::vector<gr_complex> d_fft_code_Q_A;
|
||||||
gr_complex* d_fft_code_Q_B;
|
std::vector<gr_complex> d_fft_code_Q_B;
|
||||||
gr_complex* d_inbuffer;
|
std::vector<gr_complex> d_inbuffer;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
unsigned int d_code_phase;
|
unsigned int d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitudeIA;
|
std::vector<float> d_magnitudeIA;
|
||||||
float* d_magnitudeIB;
|
std::vector<float> d_magnitudeIB;
|
||||||
float* d_magnitudeQA;
|
std::vector<float> d_magnitudeQA;
|
||||||
float* d_magnitudeQB;
|
std::vector<float> d_magnitudeQB;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
bool d_bit_transition_flag;
|
bool d_bit_transition_flag;
|
||||||
@ -247,9 +248,9 @@ private:
|
|||||||
bool d_dump;
|
bool d_dump;
|
||||||
bool d_both_signal_components;
|
bool d_both_signal_components;
|
||||||
int d_CAF_window_hz;
|
int d_CAF_window_hz;
|
||||||
float* d_CAF_vector;
|
std::vector<float> d_CAF_vector;
|
||||||
float* d_CAF_vector_I;
|
std::vector<float> d_CAF_vector_I;
|
||||||
float* d_CAF_vector_Q;
|
std::vector<float> d_CAF_vector_Q;
|
||||||
unsigned int d_channel;
|
unsigned int d_channel;
|
||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
unsigned int d_buffer_count;
|
unsigned int d_buffer_count;
|
||||||
|
@ -82,9 +82,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;
|
||||||
|
|
||||||
d_fft_code_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_A = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
|
||||||
d_fft_code_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_B = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
|
||||||
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitude = std::vector<float>(d_fft_size, 0.0F);
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
||||||
@ -99,7 +99,6 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
|
|||||||
d_doppler_resolution = 0;
|
d_doppler_resolution = 0;
|
||||||
d_threshold = 0;
|
d_threshold = 0;
|
||||||
d_doppler_step = 0;
|
d_doppler_step = 0;
|
||||||
d_grid_doppler_wipeoffs = nullptr;
|
|
||||||
d_gnss_synchro = nullptr;
|
d_gnss_synchro = nullptr;
|
||||||
d_code_phase = 0;
|
d_code_phase = 0;
|
||||||
d_doppler_freq = 0;
|
d_doppler_freq = 0;
|
||||||
@ -110,19 +109,6 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
|
|||||||
|
|
||||||
galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
|
galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
|
||||||
{
|
{
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
}
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_fft_code_A);
|
|
||||||
volk_gnsssdr_free(d_fft_code_B);
|
|
||||||
volk_gnsssdr_free(d_magnitude);
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -148,8 +134,8 @@ void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_A.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
|
|
||||||
// code B: two replicas of a primary code; the second replica is inverted.
|
// code B: two replicas of a primary code; the second replica is inverted.
|
||||||
volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
|
volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
|
||||||
@ -158,8 +144,8 @@ void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_B.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -184,16 +170,14 @@ void galileo_pcps_8ms_acquisition_cc::init()
|
|||||||
{
|
{
|
||||||
d_num_doppler_bins++;
|
d_num_doppler_bins++;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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 = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -226,7 +210,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||||
gr_vector_void_star &output_items __attribute__((unused)))
|
gr_vector_void_star &output_items __attribute__((unused)))
|
||||||
{
|
{
|
||||||
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
|
|
||||||
switch (d_state)
|
switch (d_state)
|
||||||
{
|
{
|
||||||
@ -234,7 +218,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
{
|
{
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -263,7 +247,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
float magt_A = 0.0;
|
float magt_A = 0.0;
|
||||||
float magt_B = 0.0;
|
float magt_B = 0.0;
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||||
d_input_power = 0.0;
|
d_input_power = 0.0;
|
||||||
d_mag = 0.0;
|
d_mag = 0.0;
|
||||||
@ -279,8 +263,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
<< ", doppler_step: " << d_doppler_step;
|
<< ", doppler_step: " << d_doppler_step;
|
||||||
|
|
||||||
// 1- Compute the input signal power estimation
|
// 1- Compute the input signal power estimation
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
// 2- Doppler frequency search loop
|
// 2- Doppler frequency search loop
|
||||||
@ -290,7 +274,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
|
|
||||||
volk_32fc_x2_multiply_32fc(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].data(), 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
|
||||||
@ -300,14 +284,14 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
// with the local FFT'd code A reference using SIMD operations with
|
// with the local FFT'd code A reference using SIMD operations with
|
||||||
// VOLK library
|
// VOLK library
|
||||||
volk_32fc_x2_multiply_32fc(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.data(), 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(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude.data(), d_fft_size);
|
||||||
|
|
||||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
// 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);
|
||||||
@ -316,14 +300,14 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
|||||||
// with the local FFT'd code B reference using SIMD operations with
|
// with the local FFT'd code B reference using SIMD operations with
|
||||||
// VOLK library
|
// VOLK library
|
||||||
volk_32fc_x2_multiply_32fc(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.data(), 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(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude.data(), d_fft_size);
|
||||||
|
|
||||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
// 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);
|
||||||
|
@ -41,6 +41,7 @@
|
|||||||
#include <memory>
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
class galileo_pcps_8ms_acquisition_cc;
|
class galileo_pcps_8ms_acquisition_cc;
|
||||||
|
|
||||||
@ -206,17 +207,17 @@ private:
|
|||||||
uint32_t d_well_count;
|
uint32_t d_well_count;
|
||||||
uint32_t d_fft_size;
|
uint32_t d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
uint32_t d_num_doppler_bins;
|
uint32_t d_num_doppler_bins;
|
||||||
gr_complex* d_fft_code_A;
|
std::vector<gr_complex> d_fft_code_A;
|
||||||
gr_complex* d_fft_code_B;
|
std::vector<gr_complex> d_fft_code_B;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
uint32_t d_code_phase;
|
uint32_t d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitude;
|
std::vector<float> d_magnitude;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
std::ofstream d_dump_file;
|
std::ofstream d_dump_file;
|
||||||
@ -228,4 +229,4 @@ private:
|
|||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_*/
|
#endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_ */
|
||||||
|
@ -43,6 +43,7 @@
|
|||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
|
|
||||||
extern Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
extern Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
||||||
|
|
||||||
|
|
||||||
@ -79,8 +80,7 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
|
|||||||
d_input_power = 0.0;
|
d_input_power = 0.0;
|
||||||
d_state = 0;
|
d_state = 0;
|
||||||
d_disable_assist = false;
|
d_disable_assist = false;
|
||||||
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_codes.reserve(d_fft_size);
|
||||||
d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
||||||
@ -115,8 +115,6 @@ void pcps_assisted_acquisition_cc::set_doppler_step(uint32_t doppler_step)
|
|||||||
|
|
||||||
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
|
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
|
||||||
{
|
{
|
||||||
volk_gnsssdr_free(d_carrier);
|
|
||||||
volk_gnsssdr_free(d_fft_codes);
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -156,8 +154,8 @@ 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(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -279,10 +277,10 @@ double pcps_assisted_acquisition_cc::search_maximum()
|
|||||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler].data()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler].data()), n); // write directly |abs(x)|^2 in this Doppler bin?
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -292,16 +290,14 @@ double pcps_assisted_acquisition_cc::search_maximum()
|
|||||||
|
|
||||||
float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items)
|
float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items)
|
||||||
{
|
{
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
// 1- Compute the input signal power estimation
|
// 1- Compute the input signal power estimation
|
||||||
auto *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
std::vector<float> p_tmp_vector(d_fft_size);
|
||||||
|
|
||||||
volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), in, d_fft_size);
|
||||||
|
|
||||||
const float *p_const_tmp_vector = p_tmp_vector;
|
|
||||||
float power;
|
float power;
|
||||||
volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
|
volk_32f_accumulator_s32f(&power, p_tmp_vector.data(), d_fft_size);
|
||||||
volk_gnsssdr_free(p_tmp_vector);
|
|
||||||
return (power / static_cast<float>(d_fft_size));
|
return (power / static_cast<float>(d_fft_size));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -309,7 +305,7 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
|
|||||||
int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
|
int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
|
||||||
{
|
{
|
||||||
// initialize acquisition algorithm
|
// initialize acquisition algorithm
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
|
|
||||||
DLOG(INFO) << "Channel: " << d_channel
|
DLOG(INFO) << "Channel: " << d_channel
|
||||||
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "
|
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "
|
||||||
@ -319,7 +315,7 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons
|
|||||||
<< ", doppler_step: " << d_doppler_step;
|
<< ", doppler_step: " << d_doppler_step;
|
||||||
|
|
||||||
// 2- Doppler frequency search loop
|
// 2- Doppler frequency search loop
|
||||||
auto *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
std::vector<float> p_tmp_vector(d_fft_size);
|
||||||
|
|
||||||
for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
|
for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
|
||||||
{
|
{
|
||||||
@ -332,17 +328,16 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons
|
|||||||
|
|
||||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
// 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(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
|
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes.data(), d_fft_size);
|
||||||
|
|
||||||
// compute the inverse FFT
|
// 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(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
const float *old_vector = d_grid_data[doppler_index].data();
|
const float *old_vector = d_grid_data[doppler_index].data();
|
||||||
volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector, d_fft_size);
|
volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector.data(), d_fft_size);
|
||||||
}
|
}
|
||||||
volk_gnsssdr_free(p_tmp_vector);
|
|
||||||
return d_fft_size;
|
return d_fft_size;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -215,8 +215,7 @@ private:
|
|||||||
uint32_t d_sampled_ms;
|
uint32_t d_sampled_ms;
|
||||||
uint32_t d_fft_size;
|
uint32_t d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex* d_carrier;
|
std::vector<gr_complex> d_fft_codes;
|
||||||
gr_complex* d_fft_codes;
|
|
||||||
|
|
||||||
std::vector<std::vector<float>> d_grid_data;
|
std::vector<std::vector<float>> d_grid_data;
|
||||||
std::vector<std::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
|
std::vector<std::vector<std::complex<float>>> d_grid_doppler_wipeoffs;
|
||||||
@ -239,4 +238,4 @@ private:
|
|||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/
|
#endif /* GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_ */
|
||||||
|
@ -88,13 +88,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;
|
||||||
|
|
||||||
d_fft_code_data = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_data.reserve(d_fft_size);
|
||||||
d_fft_code_pilot = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_code_pilot.reserve(d_fft_size);
|
||||||
d_data_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_data_correlation.reserve(d_fft_size);
|
||||||
d_pilot_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_pilot_correlation.reserve(d_fft_size);
|
||||||
d_correlation_plus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_correlation_plus.reserve(d_fft_size);
|
||||||
d_correlation_minus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_correlation_minus.reserve(d_fft_size);
|
||||||
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitude.reserve(d_fft_size);
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
||||||
@ -109,7 +109,6 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
|
|||||||
d_doppler_resolution = 0;
|
d_doppler_resolution = 0;
|
||||||
d_threshold = 0;
|
d_threshold = 0;
|
||||||
d_doppler_step = 0;
|
d_doppler_step = 0;
|
||||||
d_grid_doppler_wipeoffs = nullptr;
|
|
||||||
d_gnss_synchro = nullptr;
|
d_gnss_synchro = nullptr;
|
||||||
d_code_phase = 0;
|
d_code_phase = 0;
|
||||||
d_doppler_freq = 0;
|
d_doppler_freq = 0;
|
||||||
@ -120,23 +119,6 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
|
|||||||
|
|
||||||
pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
|
pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
|
||||||
{
|
{
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
}
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_fft_code_data);
|
|
||||||
volk_gnsssdr_free(d_fft_code_pilot);
|
|
||||||
volk_gnsssdr_free(d_data_correlation);
|
|
||||||
volk_gnsssdr_free(d_pilot_correlation);
|
|
||||||
volk_gnsssdr_free(d_correlation_plus);
|
|
||||||
volk_gnsssdr_free(d_correlation_minus);
|
|
||||||
volk_gnsssdr_free(d_magnitude);
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -163,16 +145,16 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> *code_data,
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_code_data, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_data.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);
|
||||||
|
|
||||||
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(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_code_pilot.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -199,15 +181,13 @@ void pcps_cccwsr_acquisition_cc::init()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Create the carrier Doppler wipeoff signals
|
// Create the carrier Doppler wipeoff signals
|
||||||
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
|
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -240,7 +220,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||||
gr_vector_void_star &output_items __attribute__((unused)))
|
gr_vector_void_star &output_items __attribute__((unused)))
|
||||||
{
|
{
|
||||||
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
|
|
||||||
switch (d_state)
|
switch (d_state)
|
||||||
{
|
{
|
||||||
@ -248,7 +228,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
{
|
{
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -277,7 +257,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
float magt_plus = 0.0;
|
float magt_plus = 0.0;
|
||||||
float magt_minus = 0.0;
|
float magt_minus = 0.0;
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
d_sample_counter += static_cast<uint64_t>(d_fft_size); // sample counter
|
d_sample_counter += static_cast<uint64_t>(d_fft_size); // sample counter
|
||||||
@ -291,19 +271,18 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
<< ", doppler_step: " << d_doppler_step;
|
<< ", doppler_step: " << d_doppler_step;
|
||||||
|
|
||||||
// 1- Compute the input signal power estimation
|
// 1- Compute the input signal power estimation
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
// 2- Doppler frequency search loop
|
// 2- Doppler frequency search loop
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
// doppler search steps
|
// doppler search steps
|
||||||
|
|
||||||
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
|
|
||||||
volk_32fc_x2_multiply_32fc(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].data(), 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
|
||||||
@ -313,27 +292,27 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
// with the local FFT'd data code reference (E1B) using SIMD operations
|
// with the local FFT'd data code reference (E1B) using SIMD operations
|
||||||
// with VOLK library
|
// with VOLK library
|
||||||
volk_32fc_x2_multiply_32fc(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.data(), d_fft_size);
|
||||||
|
|
||||||
// compute the inverse FFT
|
// compute the inverse FFT
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
|
|
||||||
// Copy the result of the correlation between wiped--off signal and data code in
|
// Copy the result of the correlation between wiped--off signal and data code in
|
||||||
// d_data_correlation.
|
// d_data_correlation.
|
||||||
memcpy(d_data_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
|
memcpy(d_data_correlation.data(), d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
|
||||||
|
|
||||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
// 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(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.data(), d_fft_size);
|
||||||
|
|
||||||
// Compute the inverse FFT
|
// Compute the inverse FFT
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
|
|
||||||
// Copy the result of the correlation between wiped--off signal and pilot code in
|
// Copy the result of the correlation between wiped--off signal and pilot code in
|
||||||
// d_data_correlation.
|
// d_data_correlation.
|
||||||
memcpy(d_pilot_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
|
memcpy(d_pilot_correlation.data(), d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
|
||||||
|
|
||||||
for (uint32_t i = 0; i < d_fft_size; i++)
|
for (uint32_t i = 0; i < d_fft_size; i++)
|
||||||
{
|
{
|
||||||
@ -346,12 +325,12 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
d_data_correlation[i].imag() - d_pilot_correlation[i].real());
|
d_data_correlation[i].imag() - d_pilot_correlation[i].real());
|
||||||
}
|
}
|
||||||
|
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_correlation_plus.data(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude.data(), d_fft_size);
|
||||||
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
|
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
|
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_correlation_minus.data(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude.data(), d_fft_size);
|
||||||
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
|
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
|
|
||||||
if (magt_plus >= magt_minus)
|
if (magt_plus >= magt_minus)
|
||||||
@ -382,10 +361,10 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
|||||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); // write directly |abs(x)|^2 in this Doppler bin?
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -46,6 +46,7 @@
|
|||||||
#include <memory>
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
|
||||||
class pcps_cccwsr_acquisition_cc;
|
class pcps_cccwsr_acquisition_cc;
|
||||||
@ -201,21 +202,21 @@ private:
|
|||||||
uint32_t d_well_count;
|
uint32_t d_well_count;
|
||||||
uint32_t d_fft_size;
|
uint32_t d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
uint32_t d_num_doppler_bins;
|
uint32_t d_num_doppler_bins;
|
||||||
gr_complex* d_fft_code_data;
|
std::vector<gr_complex> d_fft_code_data;
|
||||||
gr_complex* d_fft_code_pilot;
|
std::vector<gr_complex> d_fft_code_pilot;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
uint32_t d_code_phase;
|
uint32_t d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitude;
|
std::vector<float> d_magnitude;
|
||||||
gr_complex* d_data_correlation;
|
std::vector<gr_complex> d_data_correlation;
|
||||||
gr_complex* d_pilot_correlation;
|
std::vector<gr_complex> d_pilot_correlation;
|
||||||
gr_complex* d_correlation_plus;
|
std::vector<gr_complex> d_correlation_plus;
|
||||||
gr_complex* d_correlation_minus;
|
std::vector<gr_complex> d_correlation_minus;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
std::ofstream d_dump_file;
|
std::ofstream d_dump_file;
|
||||||
@ -227,4 +228,4 @@ private:
|
|||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/
|
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_ */
|
||||||
|
@ -112,19 +112,10 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
|
|||||||
d_in_dwell_count = 0;
|
d_in_dwell_count = 0;
|
||||||
d_cl_fft_batch_size = 1;
|
d_cl_fft_batch_size = 1;
|
||||||
|
|
||||||
d_in_buffer = new gr_complex *[d_max_dwells];
|
d_in_buffer = std::vector<std::vector<gr_complex>>(d_max_dwells, std::vector<gr_complex>(d_fft_size));
|
||||||
for (uint32_t i = 0; i < d_max_dwells; i++)
|
d_magnitude.reserve(d_fft_size);
|
||||||
{
|
d_fft_codes.reserve(d_fft_size_pow2);
|
||||||
d_in_buffer[i] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_zero_vector = std::vector<gr_complex>(d_fft_size_pow2 - d_fft_size, 0.0);
|
||||||
}
|
|
||||||
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
|
||||||
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
d_zero_vector = static_cast<gr_complex *>(volk_gnsssdr_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
for (uint32_t i = 0; i < (d_fft_size_pow2 - d_fft_size); i++)
|
|
||||||
{
|
|
||||||
d_zero_vector[i] = gr_complex(0.0, 0.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
d_opencl = init_opencl_environment("math_kernel.cl");
|
d_opencl = init_opencl_environment("math_kernel.cl");
|
||||||
|
|
||||||
@ -145,25 +136,6 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
|
|||||||
|
|
||||||
pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
|
pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
|
||||||
{
|
{
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint32_t i = 0; i < d_max_dwells; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_in_buffer[i]);
|
|
||||||
}
|
|
||||||
delete[] d_in_buffer;
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_fft_codes);
|
|
||||||
volk_gnsssdr_free(d_magnitude);
|
|
||||||
volk_gnsssdr_free(d_zero_vector);
|
|
||||||
|
|
||||||
if (d_opencl == 0)
|
if (d_opencl == 0)
|
||||||
{
|
{
|
||||||
delete d_cl_queue;
|
delete d_cl_queue;
|
||||||
@ -200,7 +172,7 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
|
|||||||
|
|
||||||
int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kernel_filename)
|
int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kernel_filename)
|
||||||
{
|
{
|
||||||
//get all platforms (drivers)
|
// get all platforms (drivers)
|
||||||
std::vector<cl::Platform> all_platforms;
|
std::vector<cl::Platform> all_platforms;
|
||||||
cl::Platform::get(&all_platforms);
|
cl::Platform::get(&all_platforms);
|
||||||
|
|
||||||
@ -210,11 +182,11 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
d_cl_platform = all_platforms[0]; //get default platform
|
d_cl_platform = all_platforms[0]; // get default platform
|
||||||
std::cout << "Using platform: " << d_cl_platform.getInfo<CL_PLATFORM_NAME>()
|
std::cout << "Using platform: " << d_cl_platform.getInfo<CL_PLATFORM_NAME>()
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
|
|
||||||
//get default GPU device of the default platform
|
// get default GPU device of the default platform
|
||||||
std::vector<cl::Device> gpu_devices;
|
std::vector<cl::Device> gpu_devices;
|
||||||
d_cl_platform.getDevices(CL_DEVICE_TYPE_GPU, &gpu_devices);
|
d_cl_platform.getDevices(CL_DEVICE_TYPE_GPU, &gpu_devices);
|
||||||
|
|
||||||
@ -239,8 +211,6 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
|
|||||||
(std::istreambuf_iterator<char>()));
|
(std::istreambuf_iterator<char>()));
|
||||||
kernel_file.close();
|
kernel_file.close();
|
||||||
|
|
||||||
// std::cout << "Kernel code: \n" << kernel_code << std::endl;
|
|
||||||
|
|
||||||
cl::Program::Sources sources;
|
cl::Program::Sources sources;
|
||||||
|
|
||||||
sources.push_back({kernel_code.c_str(), kernel_code.length()});
|
sources.push_back({kernel_code.c_str(), kernel_code.length()});
|
||||||
@ -262,10 +232,10 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(const std::string &kerne
|
|||||||
d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
|
d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
|
||||||
d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float) * d_fft_size);
|
d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float) * d_fft_size);
|
||||||
|
|
||||||
//create queue to which we will push commands for the device.
|
// create queue to which we will push commands for the device.
|
||||||
d_cl_queue = new cl::CommandQueue(d_cl_context, d_cl_device);
|
d_cl_queue = new cl::CommandQueue(d_cl_context, d_cl_device);
|
||||||
|
|
||||||
//create FFT plan
|
// create FFT plan
|
||||||
cl_int err;
|
cl_int err;
|
||||||
clFFT_Dim3 dim = {d_fft_size_pow2, 1, 1};
|
clFFT_Dim3 dim = {d_fft_size_pow2, 1, 1};
|
||||||
|
|
||||||
@ -312,7 +282,7 @@ void pcps_opencl_acquisition_cc::init()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Create the carrier Doppler wipeoff signals
|
// Create the carrier Doppler wipeoff signals
|
||||||
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
|
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
|
||||||
if (d_opencl == 0)
|
if (d_opencl == 0)
|
||||||
{
|
{
|
||||||
d_cl_buffer_grid_doppler_wipeoffs = new cl::Buffer *[d_num_doppler_bins];
|
d_cl_buffer_grid_doppler_wipeoffs = new cl::Buffer *[d_num_doppler_bins];
|
||||||
@ -320,12 +290,10 @@ void pcps_opencl_acquisition_cc::init()
|
|||||||
|
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
|
||||||
|
|
||||||
if (d_opencl == 0)
|
if (d_opencl == 0)
|
||||||
{
|
{
|
||||||
@ -334,7 +302,7 @@ void pcps_opencl_acquisition_cc::init()
|
|||||||
|
|
||||||
d_cl_queue->enqueueWriteBuffer(*(d_cl_buffer_grid_doppler_wipeoffs[doppler_index]),
|
d_cl_queue->enqueueWriteBuffer(*(d_cl_buffer_grid_doppler_wipeoffs[doppler_index]),
|
||||||
CL_TRUE, 0, sizeof(gr_complex) * d_fft_size,
|
CL_TRUE, 0, sizeof(gr_complex) * d_fft_size,
|
||||||
d_grid_doppler_wipeoffs[doppler_index]);
|
d_grid_doppler_wipeoffs[doppler_index].data());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -342,7 +310,7 @@ void pcps_opencl_acquisition_cc::init()
|
|||||||
if (d_opencl == 0)
|
if (d_opencl == 0)
|
||||||
{
|
{
|
||||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_1, CL_TRUE, sizeof(gr_complex) * d_fft_size,
|
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_1, CL_TRUE, sizeof(gr_complex) * d_fft_size,
|
||||||
sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size), d_zero_vector);
|
sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size), d_zero_vector.data());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -356,7 +324,7 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
|
|
||||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * d_fft_size,
|
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * d_fft_size,
|
||||||
sizeof(gr_complex) * (d_fft_size_pow2 - 2 * d_fft_size),
|
sizeof(gr_complex) * (d_fft_size_pow2 - 2 * d_fft_size),
|
||||||
d_zero_vector);
|
d_zero_vector.data());
|
||||||
|
|
||||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size),
|
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size),
|
||||||
sizeof(gr_complex) * d_fft_size, code);
|
sizeof(gr_complex) * d_fft_size, code);
|
||||||
@ -365,10 +333,10 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
|
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
|
||||||
0, nullptr, nullptr);
|
0, nullptr, nullptr);
|
||||||
|
|
||||||
//Conjucate the local code
|
// Conjucate the local code
|
||||||
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
|
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
|
||||||
kernel.setArg(0, *d_cl_buffer_2); //input
|
kernel.setArg(0, *d_cl_buffer_2); // input
|
||||||
kernel.setArg(1, *d_cl_buffer_fft_codes); //output
|
kernel.setArg(1, *d_cl_buffer_fft_codes); // output
|
||||||
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
|
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@ -377,8 +345,8 @@ void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -390,7 +358,6 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
|||||||
uint32_t indext = 0;
|
uint32_t indext = 0;
|
||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||||
gr_complex *in = d_in_buffer[d_well_count];
|
|
||||||
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
|
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
|
||||||
|
|
||||||
d_input_power = 0.0;
|
d_input_power = 0.0;
|
||||||
@ -405,8 +372,8 @@ 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(d_magnitude, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_in_buffer[d_well_count].data(), d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
// 2- Doppler frequency search loop
|
// 2- Doppler frequency search loop
|
||||||
@ -415,8 +382,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
|||||||
// doppler search steps
|
// doppler search steps
|
||||||
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<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(d_fft_if->get_inbuf(), d_in_buffer[d_well_count].data(),
|
||||||
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
|
d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size);
|
||||||
|
|
||||||
// 3- Perform the FFT-based convolution (parallel time search)
|
// 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
|
||||||
@ -425,14 +392,14 @@ 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(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.data(), 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(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude.data(), d_fft_size);
|
||||||
|
|
||||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
// 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);
|
||||||
@ -469,7 +436,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
|||||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||||
@ -514,14 +481,13 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
|||||||
uint32_t indext = 0;
|
uint32_t indext = 0;
|
||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why.
|
float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why.
|
||||||
gr_complex *in = d_in_buffer[d_well_count];
|
|
||||||
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
|
uint64_t samplestamp = d_sample_counter_buffer[d_well_count];
|
||||||
|
|
||||||
d_input_power = 0.0;
|
d_input_power = 0.0;
|
||||||
d_mag = 0.0;
|
d_mag = 0.0;
|
||||||
|
|
||||||
// write input vector in buffer of OpenCL device
|
// write input vector in buffer of OpenCL device
|
||||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size, in);
|
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size, d_in_buffer[d_well_count].data());
|
||||||
|
|
||||||
d_well_count++;
|
d_well_count++;
|
||||||
|
|
||||||
@ -539,8 +505,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(d_magnitude, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_in_buffer[d_well_count].data(), d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
cl::Kernel kernel;
|
cl::Kernel kernel;
|
||||||
@ -549,10 +515,9 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
|||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
// doppler search steps
|
// doppler search steps
|
||||||
|
|
||||||
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
|
|
||||||
//Multiply input signal with doppler wipe-off
|
// Multiply input signal with doppler wipe-off
|
||||||
kernel = cl::Kernel(d_cl_program, "mult_vectors");
|
kernel = cl::Kernel(d_cl_program, "mult_vectors");
|
||||||
kernel.setArg(0, *d_cl_buffer_in); //input 1
|
kernel.setArg(0, *d_cl_buffer_in); //input 1
|
||||||
kernel.setArg(1, *d_cl_buffer_grid_doppler_wipeoffs[doppler_index]); //input 2
|
kernel.setArg(1, *d_cl_buffer_grid_doppler_wipeoffs[doppler_index]); //input 2
|
||||||
@ -563,7 +528,6 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
|||||||
// In the previous operation, we store the result in the first d_fft_size positions
|
// In the previous operation, we store the result in the first d_fft_size positions
|
||||||
// of d_cl_buffer_1. The rest d_fft_size_pow2-d_fft_size already have zeros
|
// of d_cl_buffer_1. The rest d_fft_size_pow2-d_fft_size already have zeros
|
||||||
// (zero-padding is made in init() for optimization purposes).
|
// (zero-padding is made in init() for optimization purposes).
|
||||||
|
|
||||||
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
|
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
|
||||||
clFFT_Forward, (*d_cl_buffer_1)(), (*d_cl_buffer_2)(),
|
clFFT_Forward, (*d_cl_buffer_1)(), (*d_cl_buffer_2)(),
|
||||||
0, nullptr, nullptr);
|
0, nullptr, nullptr);
|
||||||
@ -592,11 +556,11 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
|||||||
// This is the only function that blocks this thread until all previously enqueued
|
// This is the only function that blocks this thread until all previously enqueued
|
||||||
// OpenCL commands are completed.
|
// OpenCL commands are completed.
|
||||||
d_cl_queue->enqueueReadBuffer(*d_cl_buffer_magnitude, CL_TRUE, 0,
|
d_cl_queue->enqueueReadBuffer(*d_cl_buffer_magnitude, CL_TRUE, 0,
|
||||||
sizeof(float) * d_fft_size, d_magnitude);
|
sizeof(float) * d_fft_size, d_magnitude.data());
|
||||||
|
|
||||||
// Search maximum
|
// 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_gnsssdr_32f_index_max_32u(&indext, d_magnitude, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude.data(), d_fft_size);
|
||||||
|
|
||||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
// 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);
|
||||||
@ -633,7 +597,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
|||||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||||
@ -705,14 +669,14 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
|
|||||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||||
gr_vector_void_star &output_items __attribute__((unused)))
|
gr_vector_void_star &output_items __attribute__((unused)))
|
||||||
{
|
{
|
||||||
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
switch (d_state)
|
switch (d_state)
|
||||||
{
|
{
|
||||||
case 0:
|
case 0:
|
||||||
{
|
{
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -742,7 +706,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
|
|||||||
uint32_t num_dwells = std::min(static_cast<int>(d_max_dwells - d_in_dwell_count), ninput_items[0]);
|
uint32_t num_dwells = std::min(static_cast<int>(d_max_dwells - d_in_dwell_count), ninput_items[0]);
|
||||||
for (uint32_t i = 0; i < num_dwells; i++)
|
for (uint32_t i = 0; i < num_dwells; i++)
|
||||||
{
|
{
|
||||||
memcpy(d_in_buffer[d_in_dwell_count++], static_cast<const gr_complex *>(input_items[i]),
|
memcpy(d_in_buffer[d_in_dwell_count++].data(), static_cast<const gr_complex *>(input_items[i]),
|
||||||
sizeof(gr_complex) * d_fft_size);
|
sizeof(gr_complex) * d_fft_size);
|
||||||
d_sample_counter += static_cast<uint64_t>(d_fft_size);
|
d_sample_counter += static_cast<uint64_t>(d_fft_size);
|
||||||
d_sample_counter_buffer.push_back(d_sample_counter);
|
d_sample_counter_buffer.push_back(d_sample_counter);
|
||||||
|
@ -241,16 +241,16 @@ private:
|
|||||||
uint32_t d_fft_size_pow2;
|
uint32_t d_fft_size_pow2;
|
||||||
int* d_max_doppler_indexs;
|
int* d_max_doppler_indexs;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
uint32_t d_num_doppler_bins;
|
uint32_t d_num_doppler_bins;
|
||||||
gr_complex* d_fft_codes;
|
std::vector<gr_complex> d_fft_codes;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
uint32_t d_code_phase;
|
uint32_t d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitude;
|
std::vector<float> d_magnitude;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
bool d_bit_transition_flag;
|
bool d_bit_transition_flag;
|
||||||
@ -261,8 +261,8 @@ private:
|
|||||||
bool d_dump;
|
bool d_dump;
|
||||||
uint32_t d_channel;
|
uint32_t d_channel;
|
||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
gr_complex* d_zero_vector;
|
std::vector<gr_complex> d_zero_vector;
|
||||||
gr_complex** d_in_buffer;
|
std::vector<std::vector<gr_complex>> d_in_buffer;
|
||||||
std::vector<uint64_t> d_sample_counter_buffer;
|
std::vector<uint64_t> d_sample_counter_buffer;
|
||||||
uint32_t d_in_dwell_count;
|
uint32_t d_in_dwell_count;
|
||||||
std::weak_ptr<ChannelFsm> d_channel_fsm;
|
std::weak_ptr<ChannelFsm> d_channel_fsm;
|
||||||
|
@ -91,19 +91,19 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
|
|||||||
d_bit_transition_flag = bit_transition_flag;
|
d_bit_transition_flag = bit_transition_flag;
|
||||||
d_folding_factor = folding_factor;
|
d_folding_factor = folding_factor;
|
||||||
|
|
||||||
//fft size is reduced.
|
// fft size is reduced.
|
||||||
d_fft_size = (d_samples_per_code) / d_folding_factor;
|
d_fft_size = (d_samples_per_code) / d_folding_factor;
|
||||||
|
|
||||||
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_codes.reserve(d_fft_size);
|
||||||
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitude.reserve(d_samples_per_code * d_folding_factor);
|
||||||
d_magnitude_folded = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitude_folded.reserve(d_fft_size);
|
||||||
|
|
||||||
d_possible_delay = new uint32_t[d_folding_factor];
|
d_possible_delay.reserve(d_folding_factor);
|
||||||
d_corr_output_f = new float[d_folding_factor];
|
d_corr_output_f.reserve(d_folding_factor);
|
||||||
|
|
||||||
/*Create the d_code signal , which would store the values of the code in its
|
/*Create the d_code signal , which would store the values of the code in its
|
||||||
original form to perform later correlation in time domain*/
|
original form to perform later correlation in time domain*/
|
||||||
d_code = new gr_complex[d_samples_per_code]();
|
d_code = std::vector<gr_complex>(d_samples_per_code, lv_cmake(0.0F, 0.0F));
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
||||||
@ -114,46 +114,22 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
|
|||||||
d_dump = dump;
|
d_dump = dump;
|
||||||
d_dump_filename = std::move(dump_filename);
|
d_dump_filename = std::move(dump_filename);
|
||||||
|
|
||||||
d_corr_acumulator = nullptr;
|
d_code_folded = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
|
||||||
d_signal_folded = nullptr;
|
d_signal_folded.reserve(d_fft_size);
|
||||||
d_code_folded = new gr_complex[d_fft_size]();
|
|
||||||
d_noise_floor_power = 0;
|
d_noise_floor_power = 0;
|
||||||
d_doppler_resolution = 0;
|
d_doppler_resolution = 0;
|
||||||
d_threshold = 0;
|
d_threshold = 0;
|
||||||
d_doppler_step = 0;
|
d_doppler_step = 0;
|
||||||
d_grid_doppler_wipeoffs = nullptr;
|
|
||||||
d_gnss_synchro = nullptr;
|
d_gnss_synchro = nullptr;
|
||||||
d_code_phase = 0;
|
d_code_phase = 0;
|
||||||
d_doppler_freq = 0;
|
d_doppler_freq = 0;
|
||||||
d_test_statistics = 0;
|
d_test_statistics = 0;
|
||||||
d_channel = 0;
|
d_channel = 0;
|
||||||
//d_code_folded = 0;
|
|
||||||
|
|
||||||
// DLOG(INFO) << "END CONSTRUCTOR";
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
|
pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
|
||||||
{
|
{
|
||||||
//DLOG(INFO) << "START DESTROYER";
|
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
}
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_fft_codes);
|
|
||||||
volk_gnsssdr_free(d_magnitude);
|
|
||||||
volk_gnsssdr_free(d_magnitude_folded);
|
|
||||||
|
|
||||||
delete d_code;
|
|
||||||
delete d_possible_delay;
|
|
||||||
delete d_corr_output_f;
|
|
||||||
delete[] d_code_folded;
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -174,16 +150,15 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
|
|||||||
|
|
||||||
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
|
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
|
||||||
{
|
{
|
||||||
/*save a local copy of the code without the folding process to perform corre-
|
/* 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.data(), code, sizeof(gr_complex) * d_samples_per_code);
|
||||||
|
|
||||||
//d_code_folded = new gr_complex[d_fft_size]();
|
memcpy(d_fft_if->get_inbuf(), d_code_folded.data(), sizeof(gr_complex) * (d_fft_size));
|
||||||
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex) * (d_fft_size));
|
|
||||||
|
|
||||||
/*perform folding of the code by the factorial factor parameter. Notice that
|
/* perform folding of the code by the factorial factor parameter. Notice that
|
||||||
folding of the code in the time stage would result in a downsampled spectrum
|
folding of the code in the time stage would result in a downsampled spectrum
|
||||||
in the frequency domain after applying the fftw operation*/
|
in the frequency domain after applying the fftw operation */
|
||||||
for (uint32_t i = 0; i < d_folding_factor; i++)
|
for (uint32_t i = 0; i < d_folding_factor; i++)
|
||||||
{
|
{
|
||||||
std::transform((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)),
|
std::transform((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)),
|
||||||
@ -193,8 +168,8 @@ void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -204,8 +179,6 @@ void pcps_quicksync_acquisition_cc::init()
|
|||||||
d_gnss_synchro->Flag_valid_symbol_output = false;
|
d_gnss_synchro->Flag_valid_symbol_output = false;
|
||||||
d_gnss_synchro->Flag_valid_pseudorange = false;
|
d_gnss_synchro->Flag_valid_pseudorange = false;
|
||||||
d_gnss_synchro->Flag_valid_word = false;
|
d_gnss_synchro->Flag_valid_word = false;
|
||||||
|
|
||||||
//DLOG(INFO) << "START init";
|
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -228,16 +201,14 @@ void pcps_quicksync_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 = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_samples_per_code * d_folding_factor));
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_samples_per_code * d_folding_factor);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_samples_per_code * d_folding_factor);
|
||||||
}
|
}
|
||||||
// DLOG(INFO) << "end init";
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -279,17 +250,16 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
* 5. Compute the test statistics and compare to the threshold
|
* 5. Compute the test statistics and compare to the threshold
|
||||||
* 6. Declare positive or negative acquisition using a message queue
|
* 6. Declare positive or negative acquisition using a message queue
|
||||||
*/
|
*/
|
||||||
//DLOG(INFO) << "START GENERAL WORK";
|
// DLOG(INFO) << "START GENERAL WORK";
|
||||||
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
//std::cout<<"general_work in quicksync gnuradio block"<<std::endl;
|
|
||||||
switch (d_state)
|
switch (d_state)
|
||||||
{
|
{
|
||||||
case 0:
|
case 0:
|
||||||
{
|
{
|
||||||
//DLOG(INFO) << "START CASE 0";
|
// DLOG(INFO) << "START CASE 0";
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -304,29 +274,27 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
|
|
||||||
d_sample_counter += static_cast<uint64_t>(d_sampled_ms * d_samples_per_ms * ninput_items[0]); // sample counter
|
d_sample_counter += static_cast<uint64_t>(d_sampled_ms * d_samples_per_ms * ninput_items[0]); // sample counter
|
||||||
consume_each(ninput_items[0]);
|
consume_each(ninput_items[0]);
|
||||||
//DLOG(INFO) << "END CASE 0";
|
// DLOG(INFO) << "END CASE 0";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case 1:
|
case 1:
|
||||||
{
|
{
|
||||||
// initialize acquisition implementing the QuickSync algorithm
|
// initialize acquisition implementing the QuickSync algorithm
|
||||||
//DLOG(INFO) << "START CASE 1";
|
// DLOG(INFO) << "START CASE 1";
|
||||||
int32_t doppler;
|
int32_t doppler;
|
||||||
uint32_t indext = 0;
|
uint32_t indext = 0;
|
||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
const auto* in = reinterpret_cast<const gr_complex*>(input_items[0]); // Get the input samples pointer
|
const auto* in = reinterpret_cast<const gr_complex*>(input_items[0]); // Get the input samples pointer
|
||||||
|
|
||||||
auto* in_temp = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
std::vector<gr_complex> in_temp(d_samples_per_code * d_folding_factor);
|
||||||
auto* in_temp_folded = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
// 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
|
||||||
auto* in_1code = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
std::vector<gr_complex> in_1code(d_samples_per_code);
|
||||||
|
|
||||||
// Stores the values of the correlation output between the local code
|
// 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
|
||||||
auto* corr_output = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
std::vector<gr_complex> corr_output(d_samples_per_code);
|
||||||
|
|
||||||
// Stores a copy of the folded version of the signal.This is used for
|
// Stores a copy of the folded version of the signal.This is used for
|
||||||
// the FFT operations in future steps of execution*/
|
// the FFT operations in future steps of execution*/
|
||||||
@ -355,8 +323,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
|
|
||||||
// 1- Compute the input signal power estimation. This operation is
|
// 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(d_magnitude, in, d_samples_per_code * d_folding_factor);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_samples_per_code * d_folding_factor);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_samples_per_code * d_folding_factor);
|
||||||
d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor);
|
d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor);
|
||||||
|
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
@ -364,8 +332,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
// Ensure that the signal is going to start with all samples
|
// 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 = std::vector<gr_complex>(d_fft_size, lv_cmake(0.0F, 0.0F));
|
||||||
memcpy(d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
|
memcpy(d_fft_if->get_inbuf(), d_signal_folded.data(), sizeof(gr_complex) * (d_fft_size));
|
||||||
|
|
||||||
// Doppler search steps and then multiplication of the incoming
|
// 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
|
||||||
@ -374,8 +342,8 @@ 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(in_temp, in,
|
volk_32fc_x2_multiply_32fc(in_temp.data(), in,
|
||||||
d_grid_doppler_wipeoffs[doppler_index],
|
d_grid_doppler_wipeoffs[doppler_index].data(),
|
||||||
d_samples_per_code * d_folding_factor);
|
d_samples_per_code * d_folding_factor);
|
||||||
|
|
||||||
// Perform folding of the carrier wiped-off incoming signal. Since
|
// Perform folding of the carrier wiped-off incoming signal. Since
|
||||||
@ -383,8 +351,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
// incoming raw data signal is of d_folding_factor^2
|
// incoming raw data signal is of d_folding_factor^2
|
||||||
for (int32_t i = 0; i < static_cast<int32_t>(d_folding_factor * d_folding_factor); i++)
|
for (int32_t i = 0; i < static_cast<int32_t>(d_folding_factor * d_folding_factor); i++)
|
||||||
{
|
{
|
||||||
std::transform((in_temp + i * d_fft_size),
|
std::transform((in_temp.data() + i * d_fft_size),
|
||||||
(in_temp + ((i + 1) * d_fft_size)),
|
(in_temp.data() + ((i + 1) * d_fft_size)),
|
||||||
d_fft_if->get_inbuf(),
|
d_fft_if->get_inbuf(),
|
||||||
d_fft_if->get_inbuf(),
|
d_fft_if->get_inbuf(),
|
||||||
std::plus<gr_complex>());
|
std::plus<gr_complex>());
|
||||||
@ -398,24 +366,22 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
// 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(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.data(), d_fft_size);
|
||||||
|
|
||||||
// compute the inverse FFT of the aliased signal
|
// compute the inverse FFT of the aliased signal
|
||||||
d_ifft->execute();
|
d_ifft->execute();
|
||||||
|
|
||||||
// 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(d_magnitude_folded,
|
volk_32fc_magnitude_squared_32f(d_magnitude_folded.data(),
|
||||||
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_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded, d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded.data(), d_fft_size);
|
||||||
|
|
||||||
magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor);
|
magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||||
|
|
||||||
delete[] d_signal_folded;
|
|
||||||
|
|
||||||
// 4- record the maximum peak and the associated synchronization parameters
|
// 4- record the maximum peak and the associated synchronization parameters
|
||||||
if (d_mag < magt)
|
if (d_mag < magt)
|
||||||
{
|
{
|
||||||
@ -443,7 +409,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
{
|
{
|
||||||
// Copy a signal of 1 code length into suggested buffer.
|
// Copy a signal of 1 code length into suggested buffer.
|
||||||
// The copied signal must have doppler effect corrected*/
|
// The copied signal must have doppler effect corrected*/
|
||||||
memcpy(in_1code, &in_temp[d_possible_delay[i]],
|
memcpy(in_1code.data(), &in_temp[d_possible_delay[i]],
|
||||||
sizeof(gr_complex) * (d_samples_per_code));
|
sizeof(gr_complex) * (d_samples_per_code));
|
||||||
|
|
||||||
// Perform multiplication of the unmodified local
|
// Perform multiplication of the unmodified local
|
||||||
@ -451,7 +417,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
// effect corrected and accumulates its value. This
|
// 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(corr_output, in_1code, d_code, d_samples_per_code);
|
volk_32fc_x2_multiply_32fc(corr_output.data(), in_1code.data(), d_code.data(), d_samples_per_code);
|
||||||
|
|
||||||
for (int32_t j = 0; j < d_samples_per_code; j++)
|
for (int32_t j = 0; j < d_samples_per_code; j++)
|
||||||
{
|
{
|
||||||
@ -459,8 +425,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Obtain maximum value of correlation given the possible delay selected
|
// Obtain maximum value of correlation given the possible delay selected
|
||||||
volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator.data(), d_folding_factor);
|
volk_32fc_magnitude_squared_32f(d_corr_output_f.data(), complex_acumulator.data(), d_folding_factor);
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f, d_folding_factor);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f.data(), d_folding_factor);
|
||||||
|
|
||||||
// Now save the real code phase in the gnss_syncro block for use in other stages
|
// Now save the real code phase in the gnss_syncro block for use in other stages
|
||||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
|
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
|
||||||
@ -483,10 +449,10 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded), n); // write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded.data()), n); // write directly |abs(x)|^2 in this Doppler bin?
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -517,10 +483,6 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
volk_gnsssdr_free(in_temp);
|
|
||||||
volk_gnsssdr_free(in_temp_folded);
|
|
||||||
volk_gnsssdr_free(in_1code);
|
|
||||||
volk_gnsssdr_free(corr_output);
|
|
||||||
consume_each(1);
|
consume_each(1);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
@ -528,7 +490,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
{
|
{
|
||||||
//DLOG(INFO) << "START CASE 2";
|
// DLOG(INFO) << "START CASE 2";
|
||||||
// 6.1- Declare positive acquisition using a message port
|
// 6.1- Declare positive acquisition using a message port
|
||||||
DLOG(INFO) << "positive acquisition";
|
DLOG(INFO) << "positive acquisition";
|
||||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||||
@ -554,13 +516,13 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
|
|
||||||
acquisition_message = 1;
|
acquisition_message = 1;
|
||||||
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
|
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
|
||||||
//DLOG(INFO) << "END CASE 2";
|
// DLOG(INFO) << "END CASE 2";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case 3:
|
case 3:
|
||||||
{
|
{
|
||||||
//DLOG(INFO) << "START CASE 3";
|
// DLOG(INFO) << "START CASE 3";
|
||||||
// 6.2- Declare negative acquisition using a message port
|
// 6.2- Declare negative acquisition using a message port
|
||||||
DLOG(INFO) << "negative acquisition";
|
DLOG(INFO) << "negative acquisition";
|
||||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||||
@ -586,7 +548,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
|||||||
|
|
||||||
acquisition_message = 2;
|
acquisition_message = 2;
|
||||||
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
|
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
|
||||||
//DLOG(INFO) << "END CASE 3";
|
// DLOG(INFO) << "END CASE 3";
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -62,6 +62,7 @@
|
|||||||
#include <functional>
|
#include <functional>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
class pcps_quicksync_acquisition_cc;
|
class pcps_quicksync_acquisition_cc;
|
||||||
|
|
||||||
@ -213,14 +214,13 @@ private:
|
|||||||
void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift,
|
void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift,
|
||||||
int32_t doppler_offset);
|
int32_t doppler_offset);
|
||||||
|
|
||||||
gr_complex* d_code;
|
std::vector<gr_complex> d_code;
|
||||||
uint32_t d_folding_factor; // also referred in the paper as 'p'
|
uint32_t d_folding_factor; // also referred in the paper as 'p'
|
||||||
float* d_corr_acumulator;
|
std::vector<uint32_t> d_possible_delay;
|
||||||
uint32_t* d_possible_delay;
|
std::vector<float> d_corr_output_f;
|
||||||
float* d_corr_output_f;
|
std::vector<float> d_magnitude_folded;
|
||||||
float* d_magnitude_folded;
|
std::vector<gr_complex> d_signal_folded;
|
||||||
gr_complex* d_signal_folded;
|
std::vector<gr_complex> d_code_folded;
|
||||||
gr_complex* d_code_folded;
|
|
||||||
float d_noise_floor_power;
|
float d_noise_floor_power;
|
||||||
int64_t d_fs_in;
|
int64_t d_fs_in;
|
||||||
int32_t d_samples_per_ms;
|
int32_t d_samples_per_ms;
|
||||||
@ -235,16 +235,16 @@ private:
|
|||||||
uint32_t d_well_count;
|
uint32_t d_well_count;
|
||||||
uint32_t d_fft_size;
|
uint32_t d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
uint32_t d_num_doppler_bins;
|
uint32_t d_num_doppler_bins;
|
||||||
gr_complex* d_fft_codes;
|
std::vector<gr_complex> d_fft_codes;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
uint32_t d_code_phase;
|
uint32_t d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitude;
|
std::vector<float> d_magnitude;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
bool d_bit_transition_flag;
|
bool d_bit_transition_flag;
|
||||||
|
@ -109,8 +109,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;
|
||||||
|
|
||||||
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
d_fft_codes.reserve(d_fft_size);
|
||||||
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
d_magnitude.reserve(d_fft_size);
|
||||||
|
|
||||||
// Direct FFT
|
// Direct FFT
|
||||||
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
d_fft_if = std::make_shared<gr::fft::fft_complex>(d_fft_size, true);
|
||||||
@ -125,8 +125,6 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
|
|||||||
d_doppler_resolution = 0;
|
d_doppler_resolution = 0;
|
||||||
d_threshold = 0;
|
d_threshold = 0;
|
||||||
d_doppler_step = 0;
|
d_doppler_step = 0;
|
||||||
d_grid_data = nullptr;
|
|
||||||
d_grid_doppler_wipeoffs = nullptr;
|
|
||||||
d_gnss_synchro = nullptr;
|
d_gnss_synchro = nullptr;
|
||||||
d_code_phase = 0;
|
d_code_phase = 0;
|
||||||
d_doppler_freq = 0;
|
d_doppler_freq = 0;
|
||||||
@ -137,20 +135,6 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
|
|||||||
|
|
||||||
pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
|
pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
|
||||||
{
|
{
|
||||||
if (d_num_doppler_bins > 0)
|
|
||||||
{
|
|
||||||
for (uint32_t i = 0; i < d_num_doppler_bins; i++)
|
|
||||||
{
|
|
||||||
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
|
|
||||||
volk_gnsssdr_free(d_grid_data[i]);
|
|
||||||
}
|
|
||||||
delete[] d_grid_doppler_wipeoffs;
|
|
||||||
delete[] d_grid_data;
|
|
||||||
}
|
|
||||||
|
|
||||||
volk_gnsssdr_free(d_fft_codes);
|
|
||||||
volk_gnsssdr_free(d_magnitude);
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -175,8 +159,8 @@ void pcps_tong_acquisition_cc::set_local_code(std::complex<float> *code)
|
|||||||
|
|
||||||
d_fft_if->execute(); // We need the FFT of local code
|
d_fft_if->execute(); // We need the FFT of local code
|
||||||
|
|
||||||
//Conjugate the local code
|
// Conjugate the local code
|
||||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
volk_32fc_conjugate_32fc(d_fft_codes.data(), d_fft_if->get_outbuf(), d_fft_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -203,23 +187,14 @@ void pcps_tong_acquisition_cc::init()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Create the carrier Doppler wipeoff signals and allocate data grid.
|
// Create the carrier Doppler wipeoff signals and allocate data grid.
|
||||||
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
|
d_grid_doppler_wipeoffs = std::vector<std::vector<gr_complex>>(d_num_doppler_bins, std::vector<gr_complex>(d_fft_size));
|
||||||
d_grid_data = new float *[d_num_doppler_bins];
|
d_grid_data = std::vector<std::vector<float>>(d_num_doppler_bins, std::vector<float>(d_fft_size, 0.0));
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
|
||||||
std::array<float, 1> _phase{};
|
std::array<float, 1> _phase{};
|
||||||
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase.data(), d_fft_size);
|
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
|
||||||
|
|
||||||
d_grid_data[doppler_index] = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
|
||||||
|
|
||||||
for (uint32_t i = 0; i < d_fft_size; i++)
|
|
||||||
{
|
|
||||||
d_grid_data[doppler_index][i] = 0;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -243,7 +218,7 @@ void pcps_tong_acquisition_cc::set_state(int32_t state)
|
|||||||
{
|
{
|
||||||
for (uint32_t i = 0; i < d_fft_size; i++)
|
for (uint32_t i = 0; i < d_fft_size; i++)
|
||||||
{
|
{
|
||||||
d_grid_data[doppler_index][i] = 0;
|
d_grid_data[doppler_index][i] = 0.0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -261,7 +236,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||||
gr_vector_void_star &output_items __attribute__((unused)))
|
gr_vector_void_star &output_items __attribute__((unused)))
|
||||||
{
|
{
|
||||||
int32_t acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
int32_t acquisition_message = -1; // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||||
|
|
||||||
switch (d_state)
|
switch (d_state)
|
||||||
{
|
{
|
||||||
@ -269,7 +244,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
{
|
{
|
||||||
if (d_active)
|
if (d_active)
|
||||||
{
|
{
|
||||||
//restart acquisition variables
|
// restart acquisition variables
|
||||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||||
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
|
||||||
@ -284,7 +259,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
{
|
{
|
||||||
for (uint32_t i = 0; i < d_fft_size; i++)
|
for (uint32_t i = 0; i < d_fft_size; i++)
|
||||||
{
|
{
|
||||||
d_grid_data[doppler_index][i] = 0;
|
d_grid_data[doppler_index][i] = 0.0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -303,7 +278,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
int32_t doppler;
|
int32_t doppler;
|
||||||
uint32_t indext = 0;
|
uint32_t indext = 0;
|
||||||
float magt = 0.0;
|
float magt = 0.0;
|
||||||
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
const auto *in = reinterpret_cast<const gr_complex *>(input_items[0]); // Get the input samples pointer
|
||||||
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||||
d_input_power = 0.0;
|
d_input_power = 0.0;
|
||||||
d_mag = 0.0;
|
d_mag = 0.0;
|
||||||
@ -319,19 +294,18 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
<< ", doppler_step: " << d_doppler_step;
|
<< ", doppler_step: " << d_doppler_step;
|
||||||
|
|
||||||
// 1- Compute the input signal power estimation
|
// 1- Compute the input signal power estimation
|
||||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size);
|
||||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
volk_32f_accumulator_s32f(&d_input_power, d_magnitude.data(), d_fft_size);
|
||||||
d_input_power /= static_cast<float>(d_fft_size);
|
d_input_power /= static_cast<float>(d_fft_size);
|
||||||
|
|
||||||
// 2- Doppler frequency search loop
|
// 2- Doppler frequency search loop
|
||||||
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||||
{
|
{
|
||||||
// doppler search steps
|
// doppler search steps
|
||||||
|
|
||||||
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||||
|
|
||||||
volk_32fc_x2_multiply_32fc(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].data(), 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
|
||||||
@ -340,24 +314,24 @@ 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(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.data(), 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(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size);
|
||||||
|
|
||||||
// Compute vector of test statistics corresponding to current doppler index.
|
// Compute vector of test statistics corresponding to current doppler index.
|
||||||
volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
|
volk_32f_s32f_multiply_32f(d_magnitude.data(), d_magnitude.data(),
|
||||||
1 / (fft_normalization_factor * fft_normalization_factor * d_input_power),
|
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(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size);
|
volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_magnitude.data(), d_grid_data[doppler_index].data(), d_fft_size);
|
||||||
|
|
||||||
// Search maximum
|
// Search maximum
|
||||||
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index], d_fft_size);
|
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index].data(), d_fft_size);
|
||||||
|
|
||||||
magt = d_grid_data[doppler_index][indext];
|
magt = d_grid_data[doppler_index][indext];
|
||||||
|
|
||||||
@ -378,10 +352,10 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
|||||||
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
|
||||||
filename.str("");
|
filename.str("");
|
||||||
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
filename << "../data/test_statistics_" << d_gnss_synchro->System
|
||||||
<< "_" << d_gnss_synchro->Signal << "_sat_"
|
<< "_" << d_gnss_synchro->Signal[0] << d_gnss_synchro->Signal[1] << "_sat_"
|
||||||
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
|
||||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
|
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); // write directly |abs(x)|^2 in this Doppler bin?
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -59,6 +59,7 @@
|
|||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
|
||||||
class pcps_tong_acquisition_cc;
|
class pcps_tong_acquisition_cc;
|
||||||
@ -220,17 +221,17 @@ private:
|
|||||||
uint32_t d_tong_max_dwells;
|
uint32_t d_tong_max_dwells;
|
||||||
uint32_t d_fft_size;
|
uint32_t d_fft_size;
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
gr_complex** d_grid_doppler_wipeoffs;
|
std::vector<std::vector<gr_complex>> d_grid_doppler_wipeoffs;
|
||||||
uint32_t d_num_doppler_bins;
|
uint32_t d_num_doppler_bins;
|
||||||
gr_complex* d_fft_codes;
|
std::vector<gr_complex> d_fft_codes;
|
||||||
float** d_grid_data;
|
std::vector<std::vector<float>> d_grid_data;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
std::shared_ptr<gr::fft::fft_complex> d_fft_if;
|
||||||
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
std::shared_ptr<gr::fft::fft_complex> d_ifft;
|
||||||
Gnss_Synchro* d_gnss_synchro;
|
Gnss_Synchro* d_gnss_synchro;
|
||||||
uint32_t d_code_phase;
|
uint32_t d_code_phase;
|
||||||
float d_doppler_freq;
|
float d_doppler_freq;
|
||||||
float d_mag;
|
float d_mag;
|
||||||
float* d_magnitude;
|
std::vector<float> d_magnitude;
|
||||||
float d_input_power;
|
float d_input_power;
|
||||||
float d_test_statistics;
|
float d_test_statistics;
|
||||||
std::ofstream d_dump_file;
|
std::ofstream d_dump_file;
|
||||||
|
Loading…
Reference in New Issue
Block a user