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

Merge branch 'next' of https://github.com/mmajoral/gnss-sdr into fpga_extended_coherent_integration

Browse Source
This commit is contained in:
Marc Majoral 2019-07-24 11:30:04 +02:00
commit ad7c1afea1
45 changed files with 472 additions and 588 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CMakeLists.txt
View File

@ -23,7 +23,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "Prevented in-tree build, it is bad practice.\nTry 'cd build && cmake ..' instead.") message(FATAL_ERROR "Prevented in-tree build, it is bad practice.\nTry 'cd build && cmake ..' instead.")
endif() endif()
cmake_minimum_required(VERSION 2.8.12...3.14.5) cmake_minimum_required(VERSION 2.8.12...3.15)
project(gnss-sdr CXX C) project(gnss-sdr CXX C)
list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/Modules) list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/Modules)
@ -682,15 +682,15 @@ set_package_properties(Boost PROPERTIES
TYPE REQUIRED TYPE REQUIRED
) )
if(CMAKE_VERSION VERSION_GREATER 3.14) if(CMAKE_VERSION VERSION_LESS 3.14)
set_package_properties(Boost PROPERTIES set(Boost_VERSION_STRING "${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION}")
DESCRIPTION "Portable C++ source libraries (found: v${Boost_VERSION_STRING})"
)
else()
set_package_properties(Boost PROPERTIES
DESCRIPTION "Portable C++ source libraries (found: v${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION})"
)
endif() endif()
if(POLICY CMP0093)
cmake_policy(SET CMP0093 NEW) # FindBoost reports Boost_VERSION in x.y.z format.
endif()
set_package_properties(Boost PROPERTIES
DESCRIPTION "Portable C++ source libraries (found: v${Boost_VERSION_STRING})"
)
if(CMAKE_VERSION VERSION_LESS 3.5) if(CMAKE_VERSION VERSION_LESS 3.5)
if(NOT TARGET Boost::boost) if(NOT TARGET Boost::boost)
@ -761,7 +761,7 @@ if(CMAKE_VERSION VERSION_LESS 3.5)
endif() endif()
# Fix for Boost Asio < 1.70 when using Clang in macOS # Fix for Boost Asio < 1.70 when using Clang in macOS
if(${Boost_VERSION} VERSION_LESS 107000) if(Boost_VERSION_STRING VERSION_LESS 1.70.0)
# Check if we have std::string_view # Check if we have std::string_view
include(CheckCXXSourceCompiles) include(CheckCXXSourceCompiles)
check_cxx_source_compiles(" check_cxx_source_compiles("

2
src/algorithms/PVT/adapters/CMakeLists.txt
View File

@ -44,7 +44,7 @@ target_include_directories(pvt_adapters
${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/interfaces
) )
if(Boost_VERSION LESS 105800) if(Boost_VERSION_STRING VERSION_LESS 1.58.0)
target_compile_definitions(pvt_adapters PRIVATE -DOLD_BOOST=1) target_compile_definitions(pvt_adapters PRIVATE -DOLD_BOOST=1)
endif() endif()

2
src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt
View File

@ -62,7 +62,7 @@ if(ENABLE_CLANG_TIDY)
endif() endif()
endif() endif()
if(Boost_VERSION LESS 105800) if(Boost_VERSION_STRING VERSION_LESS 1.58.0)
target_compile_definitions(pvt_gr_blocks PRIVATE -DOLD_BOOST=1) target_compile_definitions(pvt_gr_blocks PRIVATE -DOLD_BOOST=1)
endif() endif()

4
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -95,7 +95,7 @@ target_include_directories(pvt_libs
target_compile_definitions(pvt_libs PRIVATE -DGNSS_SDR_VERSION="${VERSION}") target_compile_definitions(pvt_libs PRIVATE -DGNSS_SDR_VERSION="${VERSION}")
if(Boost_VERSION VERSION_GREATER "106599") if(Boost_VERSION_STRING VERSION_GREATER 1.65.99)
target_compile_definitions(pvt_libs target_compile_definitions(pvt_libs
PUBLIC PUBLIC
-DBOOST_GREATER_1_65 -DBOOST_GREATER_1_65
@ -104,7 +104,7 @@ endif()
# Fix for Boost Asio < 1.70 # Fix for Boost Asio < 1.70
if(OS_IS_MACOSX) if(OS_IS_MACOSX)
if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (${Boost_VERSION} VERSION_LESS 107000)) if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (Boost_VERSION_STRING VERSION_LESS 1.70.0))
if(${has_string_view}) if(${has_string_view})
target_compile_definitions(pvt_libs target_compile_definitions(pvt_libs
PUBLIC PUBLIC

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

@ -154,6 +154,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
channel_ = 0; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr; gnss_synchro_ = nullptr;
if (in_streams_ > 1) if (in_streams_ > 1)
@ -211,6 +212,13 @@ void GalileoE1PcpsAmbiguousAcquisition::set_doppler_step(unsigned int doppler_st
acquisition_->set_doppler_step(doppler_step_); 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) 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; 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. * \brief Initializes acquisition algorithm.
*/ */
@ -176,6 +181,7 @@ private:
float threshold_; float threshold_;
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
int doppler_center_;
unsigned int sampled_ms_; unsigned int sampled_ms_;
unsigned int max_dwells_; unsigned int max_dwells_;
int64_t fs_in_; 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; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr; gnss_synchro_ = nullptr;
if (in_streams_ > 1) if (in_streams_ > 1)
@ -208,6 +209,12 @@ void GalileoE5aPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(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) 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; 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. * \brief Initializes acquisition algorithm.
*/ */
@ -169,6 +174,7 @@ private:
std::weak_ptr<ChannelFsm> channel_fsm_; std::weak_ptr<ChannelFsm> channel_fsm_;
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int sampled_ms_; unsigned int sampled_ms_;
unsigned int max_dwells_; unsigned int max_dwells_;
unsigned int in_streams_; 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; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr; gnss_synchro_ = nullptr;
if (in_streams_ > 1) if (in_streams_ > 1)
@ -200,6 +201,12 @@ void GpsL1CaPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(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) 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; 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. * \brief Initializes acquisition algorithm.
*/ */
@ -179,6 +184,7 @@ private:
float threshold_; float threshold_;
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int sampled_ms_; unsigned int sampled_ms_;
unsigned int max_dwells_; unsigned int max_dwells_;
int64_t fs_in_; 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; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr; gnss_synchro_ = nullptr;
num_codes_ = acq_parameters_.sampled_ms / acq_parameters_.ms_per_code; 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_); 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) 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; 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. * \brief Initializes acquisition algorithm.
*/ */
@ -176,6 +181,7 @@ private:
float threshold_; float threshold_;
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int max_dwells_; unsigned int max_dwells_;
int64_t fs_in_; int64_t fs_in_;
bool dump_; bool dump_;

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

@ -147,6 +147,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
channel_ = 0; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;
doppler_center_ = 0;
gnss_synchro_ = nullptr; gnss_synchro_ = nullptr;
if (in_streams_ > 1) if (in_streams_ > 1)
@ -205,6 +206,12 @@ void GpsL5iPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
acquisition_->set_doppler_step(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) 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; 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. * \brief Initializes acquisition algorithm.
*/ */
@ -176,6 +181,7 @@ private:
float threshold_; float threshold_;
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
unsigned int doppler_center_;
unsigned int max_dwells_; unsigned int max_dwells_;
int64_t fs_in_; int64_t fs_in_;
bool dump_; bool dump_;

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

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

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

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

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

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

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

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

@ -48,7 +48,6 @@
#else #else
#include <boost/filesystem/path.hpp> #include <boost/filesystem/path.hpp>
#endif #endif
#include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <matio.h> #include <matio.h>
#include <pmt/pmt.h> // for from_long #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_active = false;
d_positive_acq = 0; d_positive_acq = 0;
d_state = 0; d_state = 0;
d_old_freq = 0LL; d_doppler_bias = 0;
d_num_noncoherent_integrations_counter = 0U; 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); 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) 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_num_doppler_bins = 0U;
d_threshold = 0.0; d_threshold = 0.0;
d_doppler_step = 0U; d_doppler_step = 0U;
d_doppler_center = 0U;
d_doppler_center_step_two = 0.0; d_doppler_center_step_two = 0.0;
d_test_statistics = 0.0; d_test_statistics = 0.0;
d_channel = 0U; 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) 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 // This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid
if (is_fdma()) if (is_fdma())
{ {
@ -253,17 +251,19 @@ void pcps_acquisition::set_local_code(std::complex<float>* code)
bool pcps_acquisition::is_fdma() bool pcps_acquisition::is_fdma()
{ {
// reset the intermediate frequency
d_doppler_bias = 0;
// Dealing with FDMA system // Dealing with FDMA system
if (strcmp(d_gnss_synchro->Signal, "1G") == 0) if (strcmp(d_gnss_synchro->Signal, "1G") == 0)
{ {
d_old_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); 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_old_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; 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 true;
} }
if (strcmp(d_gnss_synchro->Signal, "2G") == 0) if (strcmp(d_gnss_synchro->Signal, "2G") == 0)
{ {
d_old_freq += DFRQ2_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); 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_old_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; 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 true;
} }
return false; 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 doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
for (uint32_t k = 0; k < d_fft_size; k++) std::fill(d_magnitude_grid[doppler_index].begin(), d_magnitude_grid[doppler_index].end(), 0.0);
{
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(gsl::span<gr_complex>(d_grid_doppler_wipeoffs[doppler_index].data(), d_fft_size), d_old_freq + doppler);
} }
update_grid_doppler_wipeoffs();
d_worker_active = false; d_worker_active = false;
if (d_dump) 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++) 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; 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_old_freq + doppler); update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], 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++) 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; 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(gsl::span<gr_complex>(d_grid_doppler_wipeoffs_step_two[doppler_index].data(), d_fft_size), d_doppler_center_step_two + doppler); update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_doppler_center_step_two + doppler);
} }
} }
@ -394,7 +390,8 @@ void pcps_acquisition::send_positive_acquisition()
<< ", code phase " << d_gnss_synchro->Acq_delay_samples << ", code phase " << d_gnss_synchro->Acq_delay_samples
<< ", doppler " << d_gnss_synchro->Acq_doppler_hz << ", doppler " << d_gnss_synchro->Acq_doppler_hz
<< ", magnitude " << d_mag << ", 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; d_positive_acq = 1;
if (!d_channel_fsm.expired()) 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; indext = index_time;
if (!d_step_two) 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 else
{ {
@ -590,7 +587,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t
if (!d_step_two) 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 else
{ {

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

@ -55,6 +55,7 @@
#include "acq_conf.h" #include "acq_conf.h"
#include "channel_fsm.h" #include "channel_fsm.h"
#include <armadillo> #include <armadillo>
#include <glog/logging.h>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/fft/fft.h> #include <gnuradio/fft/fft.h>
#include <gnuradio/gr_complex.h> // for gr_complex #include <gnuradio/gr_complex.h> // for gr_complex
@ -188,6 +189,21 @@ public:
d_doppler_step = doppler_step; 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); void set_resampler_latency(uint32_t latency_samples);
/*! /*!
@ -211,6 +227,8 @@ private:
uint32_t d_channel; uint32_t d_channel;
uint32_t d_samplesPerChip; uint32_t d_samplesPerChip;
uint32_t d_doppler_step; 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_num_noncoherent_integrations_counter;
uint32_t d_fft_size; uint32_t d_fft_size;
uint32_t d_consumed_samples; uint32_t d_consumed_samples;
@ -220,7 +238,6 @@ private:
uint32_t d_buffer_count; uint32_t d_buffer_count;
uint64_t d_sample_counter; uint64_t d_sample_counter;
int64_t d_dump_number; int64_t d_dump_number;
int64_t d_old_freq;
float d_threshold; float d_threshold;
float d_mag; float d_mag;
float d_input_power; float d_input_power;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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() void Channel::start_acquisition()
{ {
std::lock_guard<std::mutex> lk(mx); 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 stop_channel() override; //!< Stop the State Machine
void set_signal(const Gnss_Signal& gnss_signal_) override; //!< Sets the channel GNSS signal 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<AcquisitionInterface> acquisition() { return acq_; }
inline std::shared_ptr<TrackingInterface> tracking() { return trk_; } inline std::shared_ptr<TrackingInterface> tracking() { return trk_; }
inline std::shared_ptr<TelemetryDecoderInterface> telemetry() { return nav_; } inline std::shared_ptr<TelemetryDecoderInterface> telemetry() { return nav_; }

18
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Modules/VolkBoost.cmake
View File

@ -73,23 +73,21 @@ if(ENABLE_BAD_BOOST)
message(STATUS "Enabling use of known bad versions of Boost.") message(STATUS "Enabling use of known bad versions of Boost.")
endif() endif()
# For any unsuitable Boost version, add the version number below in
# the following format: XXYYZZ
# Where:
# XX is the major version ('10' for version 1)
# YY is the minor version number ('46' for 1.46)
# ZZ is the patcher version number (typically just '00')
set(Boost_NOGO_VERSIONS set(Boost_NOGO_VERSIONS
104600 104601 104700 105200 "1.46.0" "1.46.1" "1.47.0" "1.52.0"
) )
if(CMAKE_VERSION VERSION_LESS 3.14)
set(Boost_VERSION_STRING "${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION}")
endif()
foreach(ver ${Boost_NOGO_VERSIONS}) foreach(ver ${Boost_NOGO_VERSIONS})
if("${Boost_VERSION}" STREQUAL "${ver}") if("${Boost_VERSION_STRING}" STREQUAL "${ver}")
if(NOT ENABLE_BAD_BOOST) if(NOT ENABLE_BAD_BOOST)
message(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION}). Disabling.") message(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION_STRING}). Disabling.")
set(Boost_FOUND FALSE) set(Boost_FOUND FALSE)
else() else()
message(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION}). Continuing anyway.") message(STATUS "WARNING: Found a known bad version of Boost (v${Boost_VERSION_STRING}). Continuing anyway.")
set(Boost_FOUND TRUE) set(Boost_FOUND TRUE)
endif() endif()
endif() endif()

4
src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt
View File

@ -83,7 +83,7 @@ endif()
# Fix for Boost Asio < 1.70 # Fix for Boost Asio < 1.70
if(OS_IS_MACOSX) if(OS_IS_MACOSX)
if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (${Boost_VERSION} VERSION_LESS 107000)) if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (Boost_VERSION_STRING VERSION_LESS 1.70.0))
if(${has_string_view}) if(${has_string_view})
target_compile_definitions(signal_source_gr_blocks target_compile_definitions(signal_source_gr_blocks
PUBLIC PUBLIC
@ -98,7 +98,7 @@ if(OS_IS_MACOSX)
endif() endif()
endif() endif()
if(Boost_VERSION VERSION_GREATER "106599") if(Boost_VERSION_STRING VERSION_GREATER 1.65.99)
target_compile_definitions(signal_source_gr_blocks target_compile_definitions(signal_source_gr_blocks
PUBLIC PUBLIC
-DBOOST_GREATER_1_65 -DBOOST_GREATER_1_65

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, int32_t *decbits) void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits)
{ {
int32_t bit, err; int32_t bit, err;
std::array<int32_t, 4> reg{1, 1, 1, 1}; 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.data()); decode_bch15_11_01(&bitsbch[0], first_branch);
decode_bch15_11_01(&bitsbch[15], second_branch.data()); decode_bch15_11_01(&bitsbch[15], second_branch);
for (uint32_t j = 0; j < 11; j++) 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_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_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, int32_t *decbits); void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
// Preamble decoding // Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{}; 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, void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits,
int32_t *decbits) std::array<int32_t, 15> &decbits)
{ {
int32_t bit, err; int32_t bit, err;
std::array<int32_t, 4> reg{1, 1, 1, 1}; 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.data()); decode_bch15_11_01(&bitsbch[0], first_branch);
decode_bch15_11_01(&bitsbch[15], second_branch.data()); decode_bch15_11_01(&bitsbch[15], second_branch);
for (uint32_t j = 0; j < 11; j++) 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 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 Gnss_Synchro current_symbol{}; // structure to save the synchronization
// information and send the output object to the // information and send the output object to the
// next block // next block
// 1. Copy the current tracking output // 1. Copy the current tracking output
current_symbol = in[0][0]; current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue 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_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols, void decode_word(int32_t word_counter, const float *enc_word_symbols,
int32_t *dec_word_symbols); int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
// Preamble decoding // Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{}; std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};

4
src/algorithms/tracking/libs/CMakeLists.txt
View File

@ -110,7 +110,7 @@ if(NOT CMAKE_VERSION VERSION_GREATER 3.11)
) )
endif() endif()
if(Boost_VERSION VERSION_GREATER "106599") if(Boost_VERSION_STRING VERSION_GREATER 1.65.99)
target_compile_definitions(tracking_libs target_compile_definitions(tracking_libs
PUBLIC PUBLIC
-DBOOST_GREATER_1_65 -DBOOST_GREATER_1_65
@ -119,7 +119,7 @@ endif()
# Fix for Boost Asio < 1.70 # Fix for Boost Asio < 1.70
if(OS_IS_MACOSX) if(OS_IS_MACOSX)
if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (${Boost_VERSION} VERSION_LESS 107000)) if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (Boost_VERSION_STRING VERSION_LESS 1.70.0))
if(${has_string_view}) if(${has_string_view})
target_compile_definitions(tracking_libs target_compile_definitions(tracking_libs
PUBLIC PUBLIC

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

@ -62,6 +62,10 @@ public:
virtual void set_threshold(float threshold) = 0; virtual void set_threshold(float threshold) = 0;
virtual void set_doppler_max(unsigned int doppler_max) = 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_step(unsigned int doppler_step) = 0;
virtual void set_doppler_center(int doppler_center __attribute__((unused)))
{
return;
}
virtual void init() = 0; virtual void init() = 0;
virtual void set_local_code() = 0; virtual void set_local_code() = 0;
virtual void set_state(int state) = 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 gr::basic_block_sptr get_right_block() = 0;
virtual Gnss_Signal get_signal() const = 0; virtual Gnss_Signal get_signal() const = 0;
virtual void start_acquisition() = 0; virtual void start_acquisition() = 0;
virtual void assist_acquisition_doppler(double Carrier_Doppler_hz) = 0;
virtual void stop_channel() = 0; virtual void stop_channel() = 0;
virtual void set_signal(const Gnss_Signal&) = 0; virtual void set_signal(const Gnss_Signal&) = 0;
}; };

5
src/core/monitor/CMakeLists.txt
View File

@ -60,7 +60,7 @@ target_include_directories(core_monitor
) )
if(Boost_VERSION VERSION_GREATER "106599") if(Boost_VERSION_STRING VERSION_GREATER 1.65.99)
target_compile_definitions(core_monitor target_compile_definitions(core_monitor
PUBLIC PUBLIC
-DBOOST_GREATER_1_65 -DBOOST_GREATER_1_65
@ -70,7 +70,7 @@ endif()
# Fix for Boost Asio < 1.70 # Fix for Boost Asio < 1.70
if(OS_IS_MACOSX) if(OS_IS_MACOSX)
if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (${Boost_VERSION} VERSION_LESS 107000)) if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (Boost_VERSION_STRING VERSION_LESS 1.70.0))
if(${has_string_view}) if(${has_string_view})
target_compile_definitions(core_monitor target_compile_definitions(core_monitor
PUBLIC PUBLIC
@ -96,7 +96,6 @@ if(ENABLE_CLANG_TIDY)
endif() endif()
set_property(TARGET core_monitor set_property(TARGET core_monitor
APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES
$<BUILD_INTERFACE:${PROTO_INCLUDE_HEADERS}> $<BUILD_INTERFACE:${PROTO_INCLUDE_HEADERS}>

4
src/core/receiver/CMakeLists.txt
View File

@ -124,7 +124,7 @@ if(ENABLE_CUDA)
target_compile_definitions(core_receiver PRIVATE -DCUDA_GPU_ACCEL=1) target_compile_definitions(core_receiver PRIVATE -DCUDA_GPU_ACCEL=1)
endif() endif()
if(Boost_VERSION VERSION_GREATER "106599") if(Boost_VERSION_STRING VERSION_GREATER 1.65.99)
target_compile_definitions(core_receiver target_compile_definitions(core_receiver
PRIVATE PRIVATE
-DBOOST_GREATER_1_65 -DBOOST_GREATER_1_65
@ -158,7 +158,7 @@ target_link_libraries(core_receiver
# Fix for Boost Asio < 1.70 # Fix for Boost Asio < 1.70
if(OS_IS_MACOSX) if(OS_IS_MACOSX)
if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (${Boost_VERSION} VERSION_LESS 107000)) if((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (Boost_VERSION_STRING VERSION_LESS 1.70.0))
if(${has_string_view}) if(${has_string_view})
target_compile_definitions(core_receiver target_compile_definitions(core_receiver
PUBLIC PUBLIC

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

@ -1163,7 +1163,24 @@ void GNSSFlowgraph::remove_signal(const Gnss_Signal& gs)
break; 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) void GNSSFlowgraph::acquisition_manager(unsigned int who)
{ {
@ -1186,10 +1203,11 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
bool assistance_available = false; bool assistance_available = false;
bool start_acquisition = false; bool start_acquisition = false;
Gnss_Signal gnss_signal; Gnss_Signal gnss_signal;
float estimated_doppler;
double RX_time;
if (sat_ == 0) if (sat_ == 0)
{ {
float estimated_doppler;
double RX_time;
gnss_signal = search_next_signal(channels_[current_channel]->get_signal().get_signal_str(), gnss_signal = search_next_signal(channels_[current_channel]->get_signal().get_signal_str(),
true, true,
is_primary_freq, is_primary_freq,
@ -1198,12 +1216,6 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
RX_time); RX_time);
channels_[current_channel]->set_signal(gnss_signal); 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); 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 else
{ {
@ -1217,6 +1229,15 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
DLOG(INFO) << "Channel " << current_channel DLOG(INFO) << "Channel " << current_channel
<< " Starting acquisition " << channels_[current_channel]->get_signal().get_satellite() << " Starting acquisition " << channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str(); << ", 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 #ifndef ENABLE_FPGA
channels_[current_channel]->start_acquisition(); channels_[current_channel]->start_acquisition();
#else #else
@ -1232,11 +1253,6 @@ void GNSSFlowgraph::acquisition_manager(unsigned int who)
<< " secondary frequency acquisition assistance not available in " << " secondary frequency acquisition assistance not available in "
<< channels_[current_channel]->get_signal().get_satellite() << channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str(); << ", 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]; 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); std::lock_guard<std::mutex> lock(signal_list_mutex);
DLOG(INFO) << "Received " << what << " from " << who; DLOG(INFO) << "Received " << what << " from " << who;
Gnss_Signal gs = channels_[who]->get_signal();
unsigned int sat = 0; unsigned int sat = 0;
if (who < 200) if (who < 200)
{ {
@ -1287,12 +1304,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
switch (what) switch (what)
{ {
case 0: case 0:
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_[who]->get_signal().get_satellite() << ", Signal " << channels_[who]->get_signal().get_signal_str(); DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << gs.get_satellite() << ", Signal " << gs.get_signal_str();
if (sat == 0)
{
Gnss_Signal gs = channels_[who]->get_signal();
push_back_signal(gs);
}
channels_state_[who] = 0; channels_state_[who] = 0;
if (acq_channels_count_ > 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) // call the acquisition manager to assign new satellite and start next acquisition (if required)
acquisition_manager(who); 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; break;
case 1: 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. // 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; channels_state_[who] = 2;
if (acq_channels_count_ > 0) if (acq_channels_count_ > 0)
@ -1316,13 +1333,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
break; break;
case 2: 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_) if (acq_channels_count_ < max_acq_channels_)
{ {
// try to acquire the same satellite // try to acquire the same satellite
channels_state_[who] = 1; channels_state_[who] = 1;
acq_channels_count_++; 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 #ifndef ENABLE_FPGA
channels_[who]->start_acquisition(); channels_[who]->start_acquisition();
#else #else
@ -1917,7 +1934,6 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal
{ {
estimated_doppler = current_status.second->Carrier_Doppler_hz; estimated_doppler = current_status.second->Carrier_Doppler_hz;
RX_time = current_status.second->RX_time; 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 // 3. return the GPS L2 satellite and remove it from list
result = *it2; result = *it2;
if (pop) 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 push_back_signal(const Gnss_Signal& gs);
void remove_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 connected_;
bool running_; bool running_;
int sources_count_; int sources_count_;