mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-15 19:55:47 +00:00
Code Issues Releases Wiki Activity

sync

Browse Source
This commit is contained in:
Carles Fernandez 2019-07-23 19:42:52 +02:00
commit 4a229159f1
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
36 changed files with 663 additions and 334 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc
View File

@ -154,6 +154,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr;
if (in_streams_ > 1)
@ -211,6 +212,13 @@ void GalileoE1PcpsAmbiguousAcquisition::set_doppler_step(unsigned int doppler_st
acquisition_->set_doppler_step(doppler_step_);
}
void GalileoE1PcpsAmbiguousAcquisition::set_doppler_center(int doppler_center)
{
doppler_center_ = doppler_center;
acquisition_->set_doppler_center(doppler_center_);
}
void GalileoE1PcpsAmbiguousAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{

6
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h
View File

@ -123,6 +123,11 @@ public:
*/
void set_doppler_step(unsigned int doppler_step) override;
/*!
* \brief Set Doppler center for the grid search
*/
void set_doppler_center(int doppler_center) override;
/*!
* \brief Initializes acquisition algorithm.
*/
@ -176,6 +181,7 @@ private:
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
int doppler_center_;
unsigned int sampled_ms_;
unsigned int max_dwells_;
int64_t fs_in_;

7
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc
View File

@ -152,6 +152,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr;
if (in_streams_ > 1)
@ -208,6 +209,12 @@ void GalileoE5aPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(doppler_step_);
}
void GalileoE5aPcpsAcquisition::set_doppler_center(int doppler_center)
{
doppler_center_ = doppler_center;
acquisition_->set_doppler_center(doppler_center_);
}
void GalileoE5aPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{

6
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h
View File

@ -111,6 +111,11 @@ public:
*/
void set_doppler_step(unsigned int doppler_step) override;
/*!
* \brief Set Doppler center for the grid search
*/
void set_doppler_center(int doppler_center) override;
/*!
* \brief Initializes acquisition algorithm.
*/
@ -169,6 +174,7 @@ private:
std::weak_ptr<ChannelFsm> channel_fsm_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int sampled_ms_;
unsigned int max_dwells_;
unsigned int in_streams_;

7
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@ -148,6 +148,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr;
if (in_streams_ > 1)
@ -200,6 +201,12 @@ void GpsL1CaPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(doppler_step_);
}
void GpsL1CaPcpsAcquisition::set_doppler_center(int doppler_center)
{
doppler_center_ = doppler_center;
acquisition_->set_doppler_center(doppler_center_);
}
void GpsL1CaPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{

6
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h
View File

@ -127,6 +127,11 @@ public:
*/
void set_doppler_step(unsigned int doppler_step) override;
/*!
* \brief Set Doppler center for the grid search
*/
void set_doppler_center(int doppler_center) override;
/*!
* \brief Initializes acquisition algorithm.
*/
@ -179,6 +184,7 @@ private:
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int sampled_ms_;
unsigned int max_dwells_;
int64_t fs_in_;

7
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc
View File

@ -151,6 +151,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr;
num_codes_ = acq_parameters_.sampled_ms / acq_parameters_.ms_per_code;
@ -210,6 +211,12 @@ void GpsL2MPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(doppler_step_);
}
void GpsL2MPcpsAcquisition::set_doppler_center(int doppler_center)
{
doppler_center_ = doppler_center;
acquisition_->set_doppler_center(doppler_center_);
}
void GpsL2MPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{

6
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h
View File

@ -124,6 +124,11 @@ public:
*/
void set_doppler_step(unsigned int doppler_step) override;
/*!
* \brief Set Doppler center for the grid search
*/
void set_doppler_center(int doppler_center) override;
/*!
* \brief Initializes acquisition algorithm.
*/
@ -176,6 +181,7 @@ private:
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int max_dwells_;
int64_t fs_in_;
bool dump_;

7
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@ -147,6 +147,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr;
if (in_streams_ > 1)
@ -205,6 +206,12 @@ void GpsL5iPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(doppler_step_);
}
void GpsL5iPcpsAcquisition::set_doppler_center(int doppler_center)
{
doppler_center_ = doppler_center;
acquisition_->set_doppler_center(doppler_center_);
}
void GpsL5iPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{

6
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h
View File

@ -124,6 +124,11 @@ public:
*/
void set_doppler_step(unsigned int doppler_step) override;
/*!
* \brief Set Doppler center for the grid search
*/
void set_doppler_center(int doppler_center) override;
/*!
* \brief Initializes acquisition algorithm.
*/
@ -176,6 +181,7 @@ private:
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int max_dwells_;
int64_t fs_in_;
bool dump_;

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

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

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

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

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

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

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

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

37
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -48,7 +48,6 @@
#else
#include <boost/filesystem/path.hpp>
#endif
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
#include <matio.h>
#include <pmt/pmt.h> // for from_long
@ -90,7 +89,7 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
d_active = false;
d_positive_acq = 0;
d_state = 0;
d_old_freq = 0LL;
d_doppler_bias = 0;
d_num_noncoherent_integrations_counter = 0U;
d_consumed_samples = acq_parameters.sampled_ms * acq_parameters.samples_per_ms * (acq_parameters.bit_transition_flag ? 2 : 1);
if (acq_parameters.sampled_ms == acq_parameters.ms_per_code)
@ -107,6 +106,7 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
d_num_doppler_bins = 0U;
d_threshold = 0.0;
d_doppler_step = 0U;
d_doppler_center = 0U;
d_doppler_center_step_two = 0.0;
d_test_statistics = 0.0;
d_channel = 0U;
@ -215,8 +215,6 @@ void pcps_acquisition::set_resampler_latency(uint32_t latency_samples)
void pcps_acquisition::set_local_code(std::complex<float>* code)
{
// reset the intermediate frequency
d_old_freq = 0LL;
// This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid
if (is_fdma())
{
@ -253,17 +251,19 @@ void pcps_acquisition::set_local_code(std::complex<float>* code)
bool pcps_acquisition::is_fdma()
{
// reset the intermediate frequency
d_doppler_bias = 0;
// Dealing with FDMA system
if (strcmp(d_gnss_synchro->Signal, "1G") == 0)
{
d_old_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN);
LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_old_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl;
d_doppler_bias = static_cast<int32_t>(DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN));
LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_doppler_bias << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl;
return true;
}
if (strcmp(d_gnss_synchro->Signal, "2G") == 0)
{
d_old_freq += DFRQ2_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN);
LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_old_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl;
d_doppler_bias += static_cast<int32_t>(DFRQ2_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN));
LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_doppler_bias << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl;
return true;
}
return false;
@ -318,14 +318,10 @@ void pcps_acquisition::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
for (uint32_t k = 0; k < d_fft_size; k++)
{
d_magnitude_grid[doppler_index][k] = 0.0;
}
int32_t doppler = -static_cast<int32_t>(acq_parameters.doppler_max) + d_doppler_step * doppler_index;
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_old_freq + doppler);
std::fill(d_magnitude_grid[doppler_index].begin(), d_magnitude_grid[doppler_index].end(), 0.0);
}
update_grid_doppler_wipeoffs();
d_worker_active = false;
if (d_dump)
@ -341,8 +337,8 @@ void pcps_acquisition::update_grid_doppler_wipeoffs()
{
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int32_t doppler = -static_cast<int32_t>(acq_parameters.doppler_max) + d_doppler_step * doppler_index;
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_old_freq + doppler);
int32_t doppler = -static_cast<int32_t>(acq_parameters.doppler_max) + d_doppler_center + d_doppler_step * doppler_index;
update_local_carrier(gsl::span<gr_complex>(d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size), d_doppler_bias + doppler);
}
}
@ -352,7 +348,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++)
{
float doppler = (static_cast<float>(doppler_index) - static_cast<float>(floor(d_num_doppler_bins_step2 / 2.0))) * acq_parameters.doppler_step2;
update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_doppler_center_step_two + doppler);
update_local_carrier(gsl::span<gr_complex>(d_grid_doppler_wipeoffs_step_two[doppler_index].data(), d_fft_size), d_doppler_center_step_two + doppler);
}
}
@ -394,7 +390,8 @@ void pcps_acquisition::send_positive_acquisition()
<< ", code phase " << d_gnss_synchro->Acq_delay_samples
<< ", doppler " << d_gnss_synchro->Acq_doppler_hz
<< ", magnitude " << d_mag
<< ", input signal power " << d_input_power;
<< ", input signal power " << d_input_power
<< ", Assist doppler_center " << d_doppler_center;
d_positive_acq = 1;
if (!d_channel_fsm.expired())
@ -552,7 +549,7 @@ float pcps_acquisition::max_to_input_power_statistic(uint32_t& indext, int32_t&
indext = index_time;
if (!d_step_two)
{
doppler = -static_cast<int32_t>(doppler_max) + doppler_step * static_cast<int32_t>(index_doppler);
doppler = -static_cast<int32_t>(doppler_max) + d_doppler_center + doppler_step * static_cast<int32_t>(index_doppler);
}
else
{
@ -590,7 +587,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t
if (!d_step_two)
{
doppler = -static_cast<int32_t>(doppler_max) + doppler_step * static_cast<int32_t>(index_doppler);
doppler = -static_cast<int32_t>(doppler_max) + d_doppler_center + doppler_step * static_cast<int32_t>(index_doppler);
}
else
{

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

@ -55,6 +55,7 @@
#include "acq_conf.h"
#include "channel_fsm.h"
#include <armadillo>
#include <glog/logging.h>
#include <gnuradio/block.h>
#include <gnuradio/fft/fft.h>
#include <gnuradio/gr_complex.h> // for gr_complex
@ -188,6 +189,21 @@ public:
d_doppler_step = doppler_step;
}
/*!
* \brief Set Doppler center frequency for the grid search. It will refresh the Doppler grid.
* \param doppler_center - Frequency center of the search grid [Hz].
*/
inline void set_doppler_center(int32_t doppler_center)
{
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
if (doppler_center != d_doppler_center)
{
DLOG(INFO) << " Doppler assistance for Channel: " << d_channel << " => Doppler: " << doppler_center << "[Hz]";
d_doppler_center = doppler_center;
update_grid_doppler_wipeoffs();
}
}
void set_resampler_latency(uint32_t latency_samples);
/*!
@ -211,6 +227,8 @@ private:
uint32_t d_channel;
uint32_t d_samplesPerChip;
uint32_t d_doppler_step;
int32_t d_doppler_center;
int32_t d_doppler_bias;
uint32_t d_num_noncoherent_integrations_counter;
uint32_t d_fft_size;
uint32_t d_consumed_samples;
@ -220,7 +238,6 @@ private:
uint32_t d_buffer_count;
uint64_t d_sample_counter;
int64_t d_dump_number;
int64_t d_old_freq;
float d_threshold;
float d_mag;
float d_input_power;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4
src/algorithms/channel/adapters/channel.cc
View File

@ -232,6 +232,10 @@ void Channel::stop_channel()
}
void Channel::assist_acquisition_doppler(double Carrier_Doppler_hz)
{
acq_->set_doppler_center(static_cast<int>(Carrier_Doppler_hz));
}
void Channel::start_acquisition()
{
std::lock_guard<std::mutex> lk(mx);

2
src/algorithms/channel/adapters/channel.h
View File

@ -87,6 +87,8 @@ public:
void stop_channel() override; //!< Stop the State Machine
void set_signal(const Gnss_Signal& gnss_signal_) override; //!< Sets the channel GNSS signal
void assist_acquisition_doppler(double Carrier_Doppler_hz) override;
inline std::shared_ptr<AcquisitionInterface> acquisition() { return acq_; }
inline std::shared_ptr<TrackingInterface> tracking() { return trk_; }
inline std::shared_ptr<TelemetryDecoderInterface> telemetry() { return nav_; }

6
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc
View File

@ -128,7 +128,7 @@ beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs()
}
void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits)
void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, int32_t *decbits)
{
int32_t bit, err;
std::array<int32_t, 4> reg{1, 1, 1, 1};
@ -184,8 +184,8 @@ void beidou_b1i_telemetry_decoder_gs::decode_word(
}
}
decode_bch15_11_01(&bitsbch[0], first_branch);
decode_bch15_11_01(&bitsbch[15], second_branch);
decode_bch15_11_01(&bitsbch[0], first_branch.data());
decode_bch15_11_01(&bitsbch[15], second_branch.data());
for (uint32_t j = 0; j < 11; j++)
{

2
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h
View File

@ -82,7 +82,7 @@ private:
void decode_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
void decode_bch15_11_01(const int32_t *bits, int32_t *decbits);
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};

10
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc
View File

@ -129,7 +129,7 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs()
void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits,
std::array<int32_t, 15> &decbits)
int32_t *decbits)
{
int32_t bit, err;
std::array<int32_t, 4> reg{1, 1, 1, 1};
@ -185,8 +185,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word(
}
}
decode_bch15_11_01(&bitsbch[0], first_branch);
decode_bch15_11_01(&bitsbch[15], second_branch);
decode_bch15_11_01(&bitsbch[0], first_branch.data());
decode_bch15_11_01(&bitsbch[15], second_branch.data());
for (uint32_t j = 0; j < 11; j++)
{
@ -409,8 +409,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
const auto **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
Gnss_Synchro current_symbol{}; // structure to save the synchronization
// information and send the output object to the
// next block
// information and send the output object to the
// next block
// 1. Copy the current tracking output
current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue

2
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h
View File

@ -79,7 +79,7 @@ private:
void decode_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols,
int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
void decode_bch15_11_01(const int32_t *bits, int32_t *decbits);
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};

4
src/core/interfaces/acquisition_interface.h
View File

@ -62,6 +62,10 @@ public:
virtual void set_threshold(float threshold) = 0;
virtual void set_doppler_max(unsigned int doppler_max) = 0;
virtual void set_doppler_step(unsigned int doppler_step) = 0;
virtual void set_doppler_center(int doppler_center __attribute__((unused)))
{
return;
}
virtual void init() = 0;
virtual void set_local_code() = 0;
virtual void set_state(int state) = 0;

1
src/core/interfaces/channel_interface.h
View File

@ -57,6 +57,7 @@ public:
virtual gr::basic_block_sptr get_right_block() = 0;
virtual Gnss_Signal get_signal() const = 0;
virtual void start_acquisition() = 0;
virtual void assist_acquisition_doppler(double Carrier_Doppler_hz) = 0;
virtual void stop_channel() = 0;
virtual void set_signal(const Gnss_Signal&) = 0;
};

66
src/core/receiver/gnss_flowgraph.cc
View File

@ -1163,7 +1163,24 @@ void GNSSFlowgraph::remove_signal(const Gnss_Signal& gs)
break;
}
}
//project Doppler from primary frequency to secondary frequency
double GNSSFlowgraph::project_doppler(std::string searched_signal, double primary_freq_doppler_hz)
{
switch (mapStringValues_[searched_signal])
{
case evGPS_L5:
return (primary_freq_doppler_hz / FREQ1) * FREQ5;
break;
case evGAL_5X:
return (primary_freq_doppler_hz / FREQ1) * FREQ5;
break;
case evGPS_2S:
return (primary_freq_doppler_hz / FREQ1) * FREQ2;
break;
default:
return primary_freq_doppler_hz;
}
}
void GNSSFlowgraph::acquisition_manager(unsigned int who)
{
@ -1186,10 +1203,11 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
bool assistance_available = false;
bool start_acquisition = false;
Gnss_Signal gnss_signal;
float estimated_doppler;
double RX_time;
if (sat_ == 0)
{
float estimated_doppler;
double RX_time;
gnss_signal = search_next_signal(channels_[current_channel]->get_signal().get_signal_str(),
true,
is_primary_freq,
@ -1198,12 +1216,6 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
RX_time);
channels_[current_channel]->set_signal(gnss_signal);
start_acquisition = is_primary_freq or assistance_available or !configuration_->property("GNSS-SDR.assist_dual_frequency_acq", false);
// if (assistance_available)
// {
// std::cout << "Channel " << current_channel
// << " assistance available for " << channels_[current_channel]->get_signal().get_satellite()
// << ", Signal " << channels_[current_channel]->get_signal().get_signal_str() << std::endl;
// }
}
else
{
@ -1217,6 +1229,15 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
DLOG(INFO) << "Channel " << current_channel
<< " Starting acquisition " << channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str();
if (assistance_available == true and configuration_->property("GNSS-SDR.assist_dual_frequency_acq", false))
{
channels_[current_channel]->assist_acquisition_doppler(project_doppler(channels_[current_channel]->get_signal().get_signal_str(), estimated_doppler));
}
else
{
//set Doppler center to 0 Hz
channels_[current_channel]->assist_acquisition_doppler(0);
}
#ifndef ENABLE_FPGA
channels_[current_channel]->start_acquisition();
#else
@ -1232,11 +1253,6 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
<< " secondary frequency acquisition assistance not available in "
<< channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str();
// std::cout << "Channel " << current_channel
// << " secondary frequency acquisition assistance not available in "
// << channels_[current_channel]->get_signal().get_satellite()
// << ", Signal " << channels_[current_channel]->get_signal().get_signal_str() << std::endl;
}
}
DLOG(INFO) << "Channel " << current_channel << " in state " << channels_state_[current_channel];
@ -1272,6 +1288,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
std::lock_guard<std::mutex> lock(signal_list_mutex);
DLOG(INFO) << "Received " << what << " from " << who;
Gnss_Signal gs = channels_[who]->get_signal();
unsigned int sat = 0;
if (who < 200)
{
@ -1287,12 +1304,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
switch (what)
{
case 0:
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_[who]->get_signal().get_satellite() << ", Signal " << channels_[who]->get_signal().get_signal_str();
if (sat == 0)
{
Gnss_Signal gs = channels_[who]->get_signal();
push_back_signal(gs);
}
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << gs.get_satellite() << ", Signal " << gs.get_signal_str();
channels_state_[who] = 0;
if (acq_channels_count_ > 0)
{
@ -1300,11 +1312,16 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
}
// call the acquisition manager to assign new satellite and start next acquisition (if required)
acquisition_manager(who);
//push back the old signal AFTER assigning a new one to avoid selecting the same signal
if (sat == 0)
{
push_back_signal(gs);
}
break;
case 1:
DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_[who]->get_signal().get_satellite();
DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << gs.get_satellite();
// If the satellite is in the list of available ones, remove it.
remove_signal(channels_[who]->get_signal());
remove_signal(gs);
channels_state_[who] = 2;
if (acq_channels_count_ > 0)
@ -1316,13 +1333,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
break;
case 2:
DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_[who]->get_signal().get_satellite();
DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << gs.get_satellite();
if (acq_channels_count_ < max_acq_channels_)
{
// try to acquire the same satellite
channels_state_[who] = 1;
acq_channels_count_++;
DLOG(INFO) << "Channel " << who << " Starting acquisition " << channels_[who]->get_signal().get_satellite() << ", Signal " << channels_[who]->get_signal().get_signal_str();
DLOG(INFO) << "Channel " << who << " Starting acquisition " << gs.get_satellite() << ", Signal " << gs.get_signal_str();
#ifndef ENABLE_FPGA
channels_[who]->start_acquisition();
#else
@ -1917,7 +1934,6 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal
{
estimated_doppler = current_status.second->Carrier_Doppler_hz;
RX_time = current_status.second->RX_time;
// std::cout << " Channel: " << it->first << " => Doppler: " << estimated_doppler << "[Hz] \n";
// 3. return the GPS L2 satellite and remove it from list
result = *it2;
if (pop)

1
src/core/receiver/gnss_flowgraph.h
View File

@ -181,6 +181,7 @@ private:
void push_back_signal(const Gnss_Signal& gs);
void remove_signal(const Gnss_Signal& gs);
double project_doppler(std::string searched_signal, double primary_freq_doppler_hz);
bool connected_;
bool running_;
int sources_count_;